{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "99bbaa7d",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import math\n",
    "import matplotlib.pyplot as plt\n",
    "from sympy import symbols, cos, sin\n",
    "from sympy.plotting import plot_parametric\n",
    "from math import sin,cos"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9f35431a",
   "metadata": {},
   "source": [
    "# Лабораторная работа 1 , задание 2(a) , Sigma Point Kalman Filter\n",
    "Реализация сигма точечного фильтра Калмана для отслеживания координати ориентации в соответствии с odometry motion model"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "62668378",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "['ODOMETRY', '0.101373685667', '0.0996252285934', '0.000862235559071']\n",
      "['SENSOR', '1', '2.29725290252', '0.353956815014']\n",
      "['SENSOR', '2', '4.01825728983', '1.51507415344']\n",
      "['ODOMETRY', '0.101823187296', '0.0997248889496', '-0.000206654546125']\n",
      "['SENSOR', '1', '1.96846687533', '0.276209105501']\n",
      "['SENSOR', '2', '3.87986704568', '1.40393072429']\n",
      "['ODOMETRY', '0.100901695741', '0.0997582630735', '-0.000381616354179']\n",
      "['SENSOR', '1', '1.96852886749', '0.242784804425']\n",
      "['SENSOR', '2', '3.87971065887', '1.32647676387']\n",
      "['ODOMETRY', '0.0997651394437', '0.0997993534855', '-0.000208097310853']\n",
      "['SENSOR', '1', '1.87508331126', '0.0936569272975']\n",
      "['SENSOR', '2', '3.849192179', '1.20772931615']\n",
      "['ODOMETRY', '0.0988021399415', '0.100360366225', '-0.00122883669589']\n",
      "['SENSOR', '1', '1.64421872619', '-0.0593514730712']\n",
      "['SENSOR', '2', '3.95417280741', '1.25970767463']\n",
      "['ODOMETRY', '0.0998731020684', '0.0998098334361', '0.000926085589796']\n",
      "['SENSOR', '1', '1.64079501221', '-0.137553375677']\n",
      "['SENSOR', '2', '3.59759872767', '1.04090139691']\n",
      "['ODOMETRY', '0.100314651053', '0.0996200579912', '0.000575268519869']\n",
      "['SENSOR', '1', '1.63668963967', '-0.164205155951']\n",
      "['SENSOR', '2', '3.76733107867', '1.02783331425']\n",
      "['ODOMETRY', '0.100066427373', '0.0997787196014', '0.00016140029208']\n",
      "['SENSOR', '1', '1.35096574442', '-0.290175121698']\n",
      "['SENSOR', '2', '3.77521988295', '0.983315510779']\n",
      "['ODOMETRY', '0.100757002056', '0.0998428685737', '-0.000211648847469']\n",
      "['SENSOR', '8', '4.9185663896', '-0.395438658587']\n",
      "['SENSOR', '1', '1.34647237204', '-0.509755484258']\n",
      "['SENSOR', '2', '3.71427679672', '0.92294567505']\n",
      "['ODOMETRY', '0.100962422572', '0.0998534959103', '0.000658662316867']\n",
      "['SENSOR', '8', '4.87575158242', '-0.471161323013']\n",
      "['SENSOR', '1', '1.14340287924', '-0.63651274602']\n",
      "['SENSOR', '2', '3.52742272729', '0.827358592505']\n",
      "['ODOMETRY', '0.0992390051029', '0.100115677426', '-0.000216811832096']\n",
      "['SENSOR', '8', '4.72075160291', '-0.539321605004']\n",
      "['SENSOR', '1', '1.21638996495', '-0.860593555215']\n",
      "['SENSOR', '2', '3.70903844372', '0.752159761487']\n",
      "['ODOMETRY', '0.0489965903845', '0.100051905877', '-0.000257629385974']\n",
      "['SENSOR', '8', '4.86663909002', '-0.673941110611']\n",
      "['SENSOR', '1', '1.19962918841', '-0.911110534625']\n",
      "['SENSOR', '2', '3.4305673166', '0.673488499054']\n",
      "['ODOMETRY', '0.0998222275061', '0.0996351296598', '-0.000153756252698']\n",
      "['SENSOR', '8', '4.58351837652', '-0.750587339257']\n",
      "['SENSOR', '1', '1.19033672649', '-1.04401316016']\n",
      "['SENSOR', '2', '3.21903139503', '0.608609177764']\n",
      "['ODOMETRY', '0.101077500499', '0.0998498505796', '-0.000160756806792']\n",
      "['SENSOR', '8', '4.47638974681', '-0.87477261163']\n",
      "['SENSOR', '1', '0.930451391422', '-1.25663412504']\n",
      "['SENSOR', '2', '3.30661799717', '0.524107985292']\n",
      "['ODOMETRY', '0.100521808815', '0.100140214715', '-0.00102153918024']\n",
      "['SENSOR', '8', '4.42544858746', '-1.03059237397']\n",
      "['SENSOR', '1', '1.09621121037', '-1.43900397462']\n",
      "['SENSOR', '2', '3.2382990193', '0.518547924017']\n",
      "['ODOMETRY', '0.0991500821073', '0.10013162298', '0.00061821939171']\n",
      "['SENSOR', '8', '4.48032266707', '-1.16853964317']\n",
      "['SENSOR', '1', '1.05453646597', '-1.57360343045']\n",
      "['SENSOR', '2', '3.12147771546', '0.369604279194']\n",
      "['ODOMETRY', '0.000277686359907', '0.0999909476789', '-5.65207074877e-05']\n",
      "['SENSOR', '8', '4.51041519435', '-1.13505398802']\n",
      "['SENSOR', '1', '1.01408592378', '-1.6209817432']\n",
      "['SENSOR', '2', '3.0349959581', '0.307197125698']\n",
      "['ODOMETRY', '-0.000206028015542', '0.100097050614', '0.00122219552996']\n",
      "['SENSOR', '8', '4.47730873513', '-1.08933708692']\n",
      "['SENSOR', '1', '0.977458402036', '-1.79104939149']\n",
      "['SENSOR', '2', '2.7939870742', '0.344183271835']\n",
      "['ODOMETRY', '-0.000589698440506', '0.100045946005', '-1.26097962524e-05']\n",
      "['SENSOR', '8', '4.31671160573', '-1.13337841163']\n",
      "['SENSOR', '1', '0.886564013958', '-1.96817341764']\n",
      "['SENSOR', '2', '2.8062997236', '0.378866179691']\n",
      "['ODOMETRY', '0.000268189622775', '0.099944716103', '0.000225158589822']\n",
      "['SENSOR', '8', '4.44217614349', '-1.17703179109']\n",
      "['SENSOR', '1', '1.00399386332', '-1.97871776411']\n",
      "['SENSOR', '2', '2.87352149613', '0.402848100533']\n",
      "['ODOMETRY', '-0.000740364779592', '0.0999166193458', '0.000304584312929']\n",
      "['SENSOR', '8', '4.41002211254', '-1.2055058086']\n",
      "['SENSOR', '1', '1.20240484656', '-2.09055794649']\n",
      "['SENSOR', '2', '2.61257298591', '0.400689815362']\n",
      "['ODOMETRY', '-0.00093907783548', '0.0999937264589', '-0.000488136208846']\n",
      "['SENSOR', '8', '4.28220992404', '-1.22818070834']\n",
      "['SENSOR', '1', '1.35806022311', '-2.03308175218']\n",
      "['SENSOR', '2', '2.41796456399', '0.428960172356']\n",
      "['ODOMETRY', '0.000672986065937', '0.0999170760041', '9.59803128141e-05']\n",
      "['SENSOR', '8', '4.22916152226', '-1.1920443901']\n",
      "['SENSOR', '1', '1.43456173466', '-2.20151685421']\n",
      "['SENSOR', '2', '2.54268409924', '0.418576167122']\n",
      "['ODOMETRY', '-0.000190086120117', '0.0998500295775', '-0.00120829073569']\n",
      "['SENSOR', '8', '4.25479049036', '-1.29393177827']\n",
      "['SENSOR', '1', '1.3915222382', '-2.32656801851']\n",
      "['SENSOR', '2', '2.22979301015', '0.456482047365']\n",
      "['ODOMETRY', '0.000473163132143', '0.09995123511', '-0.000781756651883']\n",
      "['SENSOR', '8', '4.18161649136', '-1.28816567381']\n",
      "['SENSOR', '1', '1.3025278855', '-2.3472367671']\n",
      "['SENSOR', '2', '2.30576040901', '0.369157306239']\n",
      "['ODOMETRY', '-0.000559278187115', '0.100028723409', '9.13999205286e-06']\n",
      "['SENSOR', '8', '4.17489666144', '-1.34658482855']\n",
      "['SENSOR', '1', '1.3978512621', '-2.36482377797']\n",
      "['SENSOR', '2', '2.11259117923', '0.471244681669']\n",
      "['SENSOR', '7', '5.05551177517', '-0.919442363958']\n",
      "['ODOMETRY', '0.000257701615542', '0.10004347626', '-0.000159471500187']\n",
      "['SENSOR', '8', '3.962669916', '-1.30912369209']\n",
      "['SENSOR', '1', '1.70916168099', '-2.46045075948']\n",
      "['SENSOR', '2', '2.02595774502', '0.554005367169']\n",
      "['SENSOR', '3', '4.7371187823', '-0.173423905805']\n",
      "['SENSOR', '7', '4.82701773051', '-0.911583275309']\n",
      "['ODOMETRY', '-0.000132246838171', '0.100204785792', '0.000451918927672']\n",
      "['SENSOR', '8', '3.98259516004', '-1.43618538277']\n",
      "['SENSOR', '1', '1.79511416601', '-2.47746949194']\n",
      "['SENSOR', '2', '2.06053687043', '0.463761690139']\n",
      "['SENSOR', '3', '4.8224768036', '-0.239591007404']\n",
      "['SENSOR', '7', '4.77817974323', '-1.00970624708']\n",
      "['ODOMETRY', '0.000547071844751', '0.100153178582', '-0.00035008972123']\n",
      "['SENSOR', '8', '3.83264320308', '-1.36158444573']\n",
      "['SENSOR', '1', '1.80852245484', '-2.51982496643']\n",
      "['SENSOR', '2', '1.96102088098', '0.610994481673']\n",
      "['SENSOR', '3', '4.85211156128', '-0.1489018205']\n",
      "['SENSOR', '7', '4.71279477962', '-0.937135174382']\n",
      "['ODOMETRY', '-0.000479308539659', '0.0998432674256', '-0.000777162798183']\n",
      "['SENSOR', '8', '3.98024682642', '-1.42089195757']\n",
      "['SENSOR', '1', '1.68616417487', '-2.51982610203']\n",
      "['SENSOR', '2', '1.84497621719', '0.568224090854']\n",
      "['SENSOR', '3', '4.61852185259', '-0.195327764501']\n",
      "['SENSOR', '7', '4.6199153731', '-0.979883951883']\n",
      "['ODOMETRY', '5.14521975978e-05', '0.0998424550095', '-0.00128773957906']\n",
      "['SENSOR', '8', '4.07638490671', '-1.48596910761']\n",
      "['SENSOR', '1', '2.10918785502', '-2.53013604019']\n",
      "['SENSOR', '2', '1.87374300045', '0.596795933857']\n",
      "['SENSOR', '3', '4.63925938223', '-0.116829211894']\n",
      "['SENSOR', '7', '4.74670921432', '-1.04281914214']\n",
      "['ODOMETRY', '0.000828058608643', '0.0998754656366', '0.000718626272025']\n",
      "['SENSOR', '8', '3.98430154528', '-1.48232363932']\n",
      "['SENSOR', '1', '1.95702304621', '-2.53750111924']\n",
      "['SENSOR', '2', '1.58489922833', '0.698949835685']\n",
      "['SENSOR', '3', '4.4671595916', '-0.168854291814']\n",
      "['SENSOR', '7', '4.52930108719', '-1.12389445264']\n",
      "['ODOMETRY', '-0.000158830047539', '0.100032780056', '-0.000437720533696']\n",
      "['SENSOR', '8', '3.84481274007', '-1.42332717446']\n",
      "['SENSOR', '1', '1.79980984001', '-2.58723622781']\n",
      "['SENSOR', '2', '1.44542795818', '0.621521479992']\n",
      "['SENSOR', '3', '4.16016807253', '-0.216426375373']\n",
      "['SENSOR', '7', '4.49513851102', '-1.01028497886']\n",
      "['ODOMETRY', '0.000133544385368', '0.100021589888', '0.00109303965336']\n",
      "['SENSOR', '8', '4.07172862971', '-1.54542949541']\n",
      "['SENSOR', '1', '2.04327443852', '-2.64266309511']\n",
      "['SENSOR', '2', '1.62012370037', '0.749575235679']\n",
      "['SENSOR', '3', '4.2939412228', '-0.211337985351']\n",
      "['SENSOR', '7', '4.55765990908', '-1.01012265001']\n",
      "['ODOMETRY', '0.000256984803676', '0.100174110326', '-7.36342307491e-05']\n",
      "['SENSOR', '8', '4.06698092589', '-1.56722255044']\n",
      "['SENSOR', '1', '2.24512586768', '-2.5437818487']\n",
      "['SENSOR', '2', '1.37145664293', '0.831882746959']\n",
      "['SENSOR', '3', '4.22397677315', '-0.235036227202']\n",
      "['SENSOR', '7', '4.41926718082', '-1.00471126329']\n",
      "['ODOMETRY', '0.00058192498751', '0.0999391933134', '-0.00023129046615']\n",
      "['SENSOR', '8', '3.97991042562', '-1.60690152056']\n",
      "['SENSOR', '1', '2.35643859208', '-2.67866850272']\n",
      "['SENSOR', '2', '1.45749408492', '0.830132685583']\n",
      "['SENSOR', '3', '4.02815458891', '-0.258831937496']\n",
      "['SENSOR', '7', '4.42145324114', '-1.10938279886']\n",
      "['ODOMETRY', '0.000696732375949', '0.100156157706', '-0.000584089998119']\n",
      "['SENSOR', '8', '4.14607416122', '-1.62652743084']\n",
      "['SENSOR', '1', '2.4585060639', '-2.62149672021']\n",
      "['SENSOR', '2', '1.39609946002', '0.827146029562']\n",
      "['SENSOR', '3', '4.01022306014', '-0.221408239995']\n",
      "['SENSOR', '7', '4.26091719102', '-1.12261197694']\n",
      "['ODOMETRY', '3.6215575313e-05', '0.0999441653561', '0.000171557215833']\n",
      "['SENSOR', '8', '4.09025633382', '-1.62902600145']\n",
      "['SENSOR', '1', '2.36969383929', '-2.76059077539']\n",
      "['SENSOR', '2', '1.19862098202', '1.02746191761']\n",
      "['SENSOR', '3', '3.79856589715', '-0.166995804924']\n",
      "['SENSOR', '7', '4.28290107013', '-1.22394878975']\n",
      "['ODOMETRY', '-8.01303873469e-05', '0.100119970896', '0.000927184717075']\n",
      "['SENSOR', '8', '4.07670842571', '-1.65993610399']\n",
      "['SENSOR', '1', '2.69624348373', '-2.70745662616']\n",
      "['SENSOR', '2', '1.27863825632', '1.0228680341']\n",
      "['SENSOR', '3', '3.69329884881', '-0.310970841602']\n",
      "['SENSOR', '7', '4.33458610704', '-1.19229396722']\n",
      "['ODOMETRY', '0.000523456685193', '0.100105137199', '9.69165742264e-05']\n",
      "['SENSOR', '8', '4.19218444921', '-1.62918746421']\n",
      "['SENSOR', '1', '2.56469838469', '-2.73365039673']\n",
      "['SENSOR', '2', '1.07742306684', '1.02436797734']\n",
      "['SENSOR', '3', '3.79502409217', '-0.2864736654']\n",
      "['SENSOR', '7', '4.23474402594', '-1.08843934559']\n",
      "['ODOMETRY', '0.000760851469688', '0.0999197195975', '-0.000675084333497']\n",
      "['SENSOR', '8', '3.97471624618', '-1.67287700529']\n",
      "['SENSOR', '1', '2.75006267624', '-2.92186387986']\n",
      "['SENSOR', '2', '1.07790545646', '1.14460483582']\n",
      "['SENSOR', '3', '3.65459543213', '-0.30509704695']\n",
      "['SENSOR', '7', '4.07939470262', '-1.19849586955']\n",
      "['ODOMETRY', '0.0001776465883', '0.100000660883', '-0.000138153337743']\n",
      "['SENSOR', '8', '4.04139703382', '-1.80527111094']\n",
      "['SENSOR', '1', '2.81682858649', '-2.73778261664']\n",
      "['SENSOR', '2', '1.01157732908', '1.35009000631']\n",
      "['SENSOR', '3', '3.43641149431', '-0.177181332772']\n",
      "['SENSOR', '7', '4.1134848859', '-1.19890280774']\n",
      "['ODOMETRY', '-9.54350459669e-05', '0.100059958212', '0.000505361195257']\n",
      "['SENSOR', '8', '4.14771152129', '-1.65785363504']\n",
      "['SENSOR', '1', '2.97373538756', '-2.76749218654']\n",
      "['SENSOR', '2', '1.08572526299', '1.27905533571']\n",
      "['SENSOR', '3', '3.19721928714', '-0.285485802808']\n",
      "['SENSOR', '7', '4.30425329678', '-1.23670399341']\n",
      "['ODOMETRY', '-0.000425877997443', '0.100165010148', '0.000263981771762']\n",
      "['SENSOR', '8', '4.0209837071', '-1.67421338488']\n",
      "['SENSOR', '1', '2.98678421634', '-2.79971956357']\n",
      "['SENSOR', '2', '1.01794738917', '1.50929471669']\n",
      "['SENSOR', '3', '3.17923741534', '-0.348369090478']\n",
      "['SENSOR', '7', '4.02065742023', '-1.27095421415']\n",
      "['ODOMETRY', '6.98085102272e-05', '0.0998571294686', '0.000814819876331']\n",
      "['SENSOR', '8', '3.94704456339', '-1.84097040455']\n",
      "['SENSOR', '1', '3.00070004218', '-2.81153261862']\n",
      "['SENSOR', '2', '1.11963656291', '1.61697540263']\n",
      "['SENSOR', '3', '3.16238211291', '-0.347631273442']\n",
      "['SENSOR', '7', '4.11202372197', '-1.30111395385']\n",
      "['ODOMETRY', '-4.66038294942e-05', '0.100189586967', '-5.21580102793e-05']\n",
      "['SENSOR', '8', '4.01514296922', '-1.96395215815']\n",
      "['SENSOR', '1', '3.31125551661', '-2.81890526713']\n",
      "['SENSOR', '2', '1.08993866286', '1.69673860324']\n",
      "['SENSOR', '3', '3.16301966354', '-0.29879355839']\n",
      "['SENSOR', '7', '4.0697079487', '-1.2813948975']\n",
      "['ODOMETRY', '-1.1888042286e-05', '0.0999378556007', '0.000874157595222']\n",
      "['SENSOR', '8', '4.2194758592', '-1.87684781408']\n",
      "['SENSOR', '1', '3.38916611019', '-2.79022010215']\n",
      "['SENSOR', '2', '0.953931009281', '1.74716622466']\n",
      "['SENSOR', '3', '2.97789976956', '-0.385301687676']\n",
      "['SENSOR', '7', '3.99090383462', '-1.29098027348']\n",
      "['ODOMETRY', '-0.000548398686482', '0.0999074163285', '-0.000586249282159']\n",
      "['SENSOR', '8', '4.01544662734', '-1.8776296021']\n",
      "['SENSOR', '1', '3.58685644655', '-2.88216804']\n",
      "['SENSOR', '2', '1.09959994011', '1.88380797133']\n",
      "['SENSOR', '3', '3.10144999253', '-0.263328469751']\n",
      "['SENSOR', '7', '4.13471788936', '-1.42326420591']\n",
      "['ODOMETRY', '0.000209771470498', '0.0999383153862', '0.000442898738476']\n",
      "['SENSOR', '8', '4.11908889453', '-1.84906255817']\n",
      "['SENSOR', '1', '3.40262134444', '-2.79577392772']\n",
      "['SENSOR', '2', '0.98313589166', '1.93949068611']\n",
      "['SENSOR', '3', '2.76424553473', '-0.353284737573']\n",
      "['SENSOR', '7', '4.04514369826', '-1.34914177025']\n",
      "['ODOMETRY', '-0.000636488566826', '0.0999395698257', '0.000643908734646']\n",
      "['SENSOR', '8', '4.26368708251', '-1.95171561203']\n",
      "['SENSOR', '1', '3.59611549468', '-2.8643885436']\n",
      "['SENSOR', '2', '1.25741341306', '2.05874595093']\n",
      "['SENSOR', '3', '2.62985116185', '-0.340176489157']\n",
      "['SENSOR', '7', '4.0715354283', '-1.34078750934']\n",
      "['ODOMETRY', '-0.000749506228593', '0.0998919597135', '0.000431194734685']\n",
      "['SENSOR', '8', '4.21795338302', '-1.92674399779']\n",
      "['SENSOR', '1', '3.64715670833', '-2.88849823757']\n",
      "['SENSOR', '2', '1.15505103131', '2.09289492551']\n",
      "['SENSOR', '3', '2.52001295552', '-0.472631064422']\n",
      "['SENSOR', '7', '3.95528847465', '-1.41585544885']\n",
      "['ODOMETRY', '0.000406907201319', '0.0998586394002', '4.54817335274e-05']\n",
      "['SENSOR', '8', '4.36749740434', '-2.0474823273']\n",
      "['SENSOR', '1', '3.68174071666', '-2.9201773312']\n",
      "['SENSOR', '2', '1.22696609171', '2.26296774635']\n",
      "['SENSOR', '3', '2.41868212361', '-0.336817224032']\n",
      "['SENSOR', '7', '3.98911047334', '-1.47028485405']\n",
      "['ODOMETRY', '0.00020261199028', '0.0999232742711', '-0.00079340859659']\n",
      "['SENSOR', '8', '4.3737723756', '-1.93110738924']\n",
      "['SENSOR', '1', '3.8479658409', '-2.96461382002']\n",
      "['SENSOR', '2', '1.44650342879', '2.19184285471']\n",
      "['SENSOR', '3', '2.4378797797', '-0.376196945898']\n",
      "['SENSOR', '7', '3.99144004155', '-1.53192738274']\n",
      "['ODOMETRY', '0.000476456640183', '0.0999946391606', '0.000673997332114']\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "['SENSOR', '8', '4.18501121114', '-1.96001604794']\n",
      "['SENSOR', '1', '3.94177735119', '-2.84551297654']\n",
      "['SENSOR', '2', '1.18038634345', '2.41547551073']\n",
      "['SENSOR', '3', '2.25386927139', '-0.458608006904']\n",
      "['SENSOR', '7', '4.01204561994', '-1.48975256344']\n",
      "['ODOMETRY', '0.000136092271998', '0.0999996467097', '-0.000522848477433']\n",
      "['SENSOR', '8', '4.40946468491', '-2.02094252774']\n",
      "['SENSOR', '1', '3.91922179189', '-2.87072202395']\n",
      "['SENSOR', '2', '1.44580310453', '2.32508479098']\n",
      "['SENSOR', '3', '2.15756119315', '-0.424067573571']\n",
      "['SENSOR', '7', '4.06104474419', '-1.57224455589']\n",
      "['ODOMETRY', '0.000302269878199', '0.0999353432319', '0.000539895064198']\n",
      "['SENSOR', '8', '4.5165550898', '-2.00275541219']\n",
      "['SENSOR', '1', '4.25083690758', '-2.88476512783']\n",
      "['SENSOR', '2', '1.59664742666', '2.36949118143']\n",
      "['SENSOR', '3', '2.18656915806', '-0.37303565508']\n",
      "['SENSOR', '7', '4.05007373159', '-1.58049943139']\n",
      "['ODOMETRY', '0.00013361527452', '0.100074513275', '0.000560546767954']\n",
      "['SENSOR', '8', '4.4359853347', '-1.98886105814']\n",
      "['SENSOR', '1', '4.34929216262', '-2.84640872927']\n",
      "['SENSOR', '2', '1.51357200054', '2.46096763847']\n",
      "['SENSOR', '3', '2.1177123351', '-0.433574921219']\n",
      "['SENSOR', '7', '3.63185364294', '-1.65552297298']\n",
      "['ODOMETRY', '-0.000748347401324', '0.0999214044639', '0.000183054107003']\n",
      "['SENSOR', '8', '4.33976493299', '-2.0786008695']\n",
      "['SENSOR', '1', '4.26489841524', '-2.98062522043']\n",
      "['SENSOR', '2', '1.61966127202', '2.46647468873']\n",
      "['SENSOR', '3', '2.02261768412', '-0.484039818089']\n",
      "['SENSOR', '7', '4.09245647518', '-1.63859759267']\n",
      "['ODOMETRY', '0.000428845906387', '0.10008081915', '0.000352531758719']\n",
      "['SENSOR', '8', '4.66591979904', '-2.11042811091']\n",
      "['SENSOR', '1', '4.5400120979', '-2.94675157545']\n",
      "['SENSOR', '2', '1.66151980155', '2.4772319982']\n",
      "['SENSOR', '3', '1.69406105599', '-0.528339655367']\n",
      "['SENSOR', '7', '3.93440624897', '-1.69607534156']\n",
      "['ODOMETRY', '0.000241280703103', '0.100190717382', '0.00021908908565']\n",
      "['SENSOR', '8', '4.65394245626', '-2.10046665109']\n",
      "['SENSOR', '1', '4.6922590945', '-2.8776113437']\n",
      "['SENSOR', '2', '1.84720641205', '2.51332375216']\n",
      "['SENSOR', '3', '1.7878592279', '-0.6464169453']\n",
      "['SENSOR', '7', '4.08108369627', '-1.67639330679']\n",
      "['ODOMETRY', '-0.00119608488614', '0.0998300419662', '-0.000785692437084']\n",
      "['SENSOR', '8', '4.77404605226', '-2.08170107067']\n",
      "['SENSOR', '1', '4.33976416105', '-2.90292850512']\n",
      "['SENSOR', '2', '1.88969783614', '2.58825155721']\n",
      "['SENSOR', '3', '1.73764363829', '-0.49745195748']\n",
      "['SENSOR', '7', '3.96406958388', '-1.66060095497']\n",
      "['ODOMETRY', '-0.000283673180471', '0.0998692948249', '-6.45801742076e-05']\n",
      "['SENSOR', '8', '4.72928958355', '-2.11808295378']\n",
      "['SENSOR', '1', '4.85446874563', '-2.96828803681']\n",
      "['SENSOR', '2', '1.94629939838', '2.61814643875']\n",
      "['SENSOR', '3', '1.60770645745', '-0.563753684955']\n",
      "['SENSOR', '7', '3.89182668135', '-1.78660920606']\n",
      "['ODOMETRY', '-0.000182066224786', '0.0998742815469', '-0.000565724685502']\n",
      "['SENSOR', '8', '4.66229866868', '-2.20515623143']\n",
      "['SENSOR', '1', '4.80809797253', '-3.00294348241']\n",
      "['SENSOR', '2', '2.13430608854', '2.71645106462']\n",
      "['SENSOR', '3', '1.376952877', '-0.663110570535']\n",
      "['SENSOR', '7', '3.8811618647', '-1.8038777696']\n",
      "['ODOMETRY', '0.000446460674214', '0.0998339319617', '0.000172792242606']\n",
      "['SENSOR', '8', '5.02546804585', '-2.19510092046']\n",
      "['SENSOR', '1', '5.01349268035', '-2.96140277102']\n",
      "['SENSOR', '2', '2.16725029068', '2.71920290634']\n",
      "['SENSOR', '3', '1.6217305118', '-0.701135122275']\n",
      "['SENSOR', '7', '3.94079630166', '-1.73582055105']\n",
      "['ODOMETRY', '-0.000192785040713', '0.099962525761', '-0.000198322747385']\n",
      "['SENSOR', '8', '4.94083414559', '-2.11537295171']\n",
      "['SENSOR', '9', '4.91446231227', '-0.887651756022']\n",
      "['SENSOR', '2', '2.16197834316', '2.69904984308']\n",
      "['SENSOR', '3', '1.525427237', '-0.711047076592']\n",
      "['SENSOR', '7', '4.01530297276', '-1.90341160103']\n",
      "['ODOMETRY', '-0.00041677675012', '0.0998935259811', '-0.000185627166787']\n",
      "['SENSOR', '8', '4.9944171849', '-2.22316600512']\n",
      "['SENSOR', '9', '4.91770926354', '-0.875068268808']\n",
      "['SENSOR', '2', '2.37108525182', '2.63893274328']\n",
      "['SENSOR', '3', '1.22148996766', '-0.779993952297']\n",
      "['SENSOR', '7', '4.05942517061', '-1.80602776182']\n",
      "['ODOMETRY', '-0.000504187302557', '0.099935062112', '-0.000102208043102']\n",
      "['SENSOR', '9', '5.05113586384', '-0.939362710755']\n",
      "['SENSOR', '2', '2.42075573102', '2.65401889272']\n",
      "['SENSOR', '3', '1.40673857311', '-0.792218436955']\n",
      "['SENSOR', '7', '4.01097220078', '-1.86268980877']\n",
      "['ODOMETRY', '0.00010208423308', '0.10009595201', '0.000112598536447']\n",
      "['SENSOR', '9', '4.80175052631', '-0.901315690154']\n",
      "['SENSOR', '2', '2.53903797695', '2.78425180019']\n",
      "['SENSOR', '3', '1.21005538358', '-0.947107299591']\n",
      "['SENSOR', '7', '4.21526740137', '-1.77072934034']\n",
      "['ODOMETRY', '-0.000470079132191', '0.100071592578', '0.000364741934559']\n",
      "['SENSOR', '9', '4.8930608011', '-0.931458155021']\n",
      "['SENSOR', '2', '2.49064672217', '2.70820827557']\n",
      "['SENSOR', '3', '1.1298112679', '-0.927638881263']\n",
      "['SENSOR', '7', '4.10863769033', '-1.85570810456']\n",
      "['ODOMETRY', '0.000293614872845', '0.0999543517986', '0.000235684154045']\n",
      "['SENSOR', '9', '4.78120287936', '-0.931491845794']\n",
      "['SENSOR', '2', '2.65158902922', '2.74003752891']\n",
      "['SENSOR', '3', '1.13696167911', '-1.02091756573']\n",
      "['SENSOR', '7', '4.14832981289', '-1.93565577604']\n",
      "['ODOMETRY', '-8.40851246277e-05', '0.0999403556225', '-0.000176595962094']\n",
      "['SENSOR', '9', '4.57621416752', '-0.993635910976']\n",
      "['SENSOR', '2', '2.92434868722', '2.7082657163']\n",
      "['SENSOR', '3', '0.871667628237', '-1.08932996938']\n",
      "['SENSOR', '7', '4.2336588264', '-1.9442154823']\n",
      "['ODOMETRY', '-0.00962491243249', '0.100226987653', '-0.0205912870107']\n",
      "['SENSOR', '9', '4.44837110071', '-0.989307488326']\n",
      "['SENSOR', '2', '2.8892743732', '2.84845502425']\n",
      "['SENSOR', '3', '0.891192383698', '-1.23984271']\n",
      "['SENSOR', '7', '4.24686986677', '-1.92323203717']\n",
      "['ODOMETRY', '-0.0102033452948', '0.0998839630755', '-0.0215571904424']\n",
      "['SENSOR', '9', '4.51127976731', '-0.965675234448']\n",
      "['SENSOR', '2', '2.99371450745', '2.84262811823']\n",
      "['SENSOR', '3', '0.799137718613', '-1.19111999767']\n",
      "['SENSOR', '7', '4.34718168559', '-1.87200596541']\n",
      "['ODOMETRY', '-0.00860842707999', '0.0998102570683', '-0.0193391675193']\n",
      "['SENSOR', '9', '4.45640142154', '-0.978412228293']\n",
      "['SENSOR', '2', '3.26617603062', '2.8384001977']\n",
      "['SENSOR', '3', '0.816222604143', '-1.30597066763']\n",
      "['SENSOR', '7', '4.2980455787', '-1.89648658794']\n",
      "['ODOMETRY', '-0.00956022288558', '0.100150972292', '-0.0200463224548']\n",
      "['SENSOR', '9', '4.37573768277', '-0.917844889679']\n",
      "['SENSOR', '2', '3.317865598', '2.95007305006']\n",
      "['SENSOR', '3', '0.817247142217', '-1.37869382939']\n",
      "['SENSOR', '7', '4.09119325406', '-1.94192716645']\n",
      "['ODOMETRY', '-0.0103002834124', '0.100007766723', '-0.0197368000329']\n",
      "['SENSOR', '9', '4.21811509958', '-0.851878267922']\n",
      "['SENSOR', '2', '3.18846120949', '2.97202181201']\n",
      "['SENSOR', '3', '1.13340957174', '-1.46851281333']\n",
      "['SENSOR', '7', '4.38585468217', '-1.82603901749']\n",
      "['ODOMETRY', '-0.0102831048513', '0.10010953823', '-0.0202530209795']\n",
      "['SENSOR', '9', '4.26699097617', '-0.944178359109']\n",
      "['SENSOR', '2', '3.38023967491', '3.01452503314']\n",
      "['SENSOR', '3', '0.720668136763', '-1.55711368901']\n",
      "['SENSOR', '7', '4.33394842239', '-1.91875382264']\n",
      "['ODOMETRY', '-0.0105866501206', '0.0999017879145', '-0.0197382893367']\n",
      "['SENSOR', '9', '4.22270883023', '-0.860892437552']\n",
      "['SENSOR', '2', '3.36042267996', '3.01995506444']\n",
      "['SENSOR', '3', '0.717148685032', '-1.66529431909']\n",
      "['SENSOR', '7', '4.43948581701', '-1.84649833616']\n",
      "['ODOMETRY', '-0.0113245534652', '0.100123041114', '-0.0190696769167']\n",
      "['SENSOR', '9', '4.16843098657', '-0.834433300637']\n",
      "['SENSOR', '2', '3.51256815216', '2.97921133252']\n",
      "['SENSOR', '3', '0.834830514938', '-1.73903910734']\n",
      "['SENSOR', '7', '4.28359501376', '-1.86953074931']\n",
      "['ODOMETRY', '-0.00971082754926', '0.100025950554', '-0.0198211377492']\n",
      "['SENSOR', '9', '4.02856295399', '-0.968273518321']\n",
      "['SENSOR', '2', '3.63658640677', '3.12915861055']\n",
      "['SENSOR', '3', '0.734021188267', '-1.75470305372']\n",
      "['SENSOR', '7', '4.54650380966', '-1.76609368488']\n",
      "['ODOMETRY', '-0.010589475487', '0.10010496938', '-0.0199922914546']\n",
      "['SENSOR', '9', '3.91015885795', '-0.869086963006']\n",
      "['SENSOR', '2', '3.87255632648', '3.16604194771']\n",
      "['SENSOR', '3', '0.865196059921', '-1.86949836389']\n",
      "['SENSOR', '7', '4.51133867781', '-1.81430214087']\n",
      "['ODOMETRY', '-0.00946128189441', '0.0997349532468', '-0.0199718063448']\n",
      "['SENSOR', '9', '3.82613593432', '-0.874876680077']\n",
      "['SENSOR', '2', '3.83879660579', '-3.13229830775']\n",
      "['SENSOR', '3', '1.06514315073', '-1.87293500873']\n",
      "['SENSOR', '7', '4.56299343999', '-1.75934253741']\n",
      "['ODOMETRY', '-0.00979951516687', '0.0997189457445', '-0.0206080614093']\n",
      "['SENSOR', '9', '4.12881342913', '-0.867651605124']\n",
      "['SENSOR', '2', '4.0288094419', '-3.2276561161']\n",
      "['SENSOR', '3', '0.901200006923', '-2.03251122022']\n",
      "['SENSOR', '7', '4.62001856502', '-1.8309460386']\n",
      "['ODOMETRY', '-0.00943323576827', '0.100124414548', '-0.0203296697597']\n",
      "['SENSOR', '9', '3.70232136893', '-0.94416511906']\n",
      "['SENSOR', '2', '3.98368634906', '-3.07682994555']\n",
      "['SENSOR', '3', '0.899772694406', '-2.09221030348']\n",
      "['SENSOR', '7', '4.46139118814', '-1.86311006595']\n",
      "['ODOMETRY', '-0.0100890424076', '0.0999779958111', '-0.0198161117798']\n",
      "['SENSOR', '9', '3.7518939771', '-0.918292146011']\n",
      "['SENSOR', '2', '4.21633761598', '-3.02832665334']\n",
      "['SENSOR', '3', '1.18763314519', '-2.0979866523']\n",
      "['SENSOR', '7', '4.57848442895', '-1.89875432575']\n",
      "['ODOMETRY', '-0.00985362326451', '0.100067538531', '-0.0193174898999']\n",
      "['SENSOR', '9', '3.59551880688', '-0.808067235008']\n",
      "['SENSOR', '2', '4.17678928397', '-2.9234396319']\n",
      "['SENSOR', '3', '1.07794432908', '-2.21510076284']\n",
      "['SENSOR', '7', '4.51763234841', '-1.82762500095']\n",
      "['ODOMETRY', '-0.0106424165584', '0.0998962699375', '-0.0193739099135']\n",
      "['SENSOR', '9', '3.54948085637', '-0.797013500387']\n",
      "['SENSOR', '2', '4.28131949654', '-2.97537943488']\n",
      "['SENSOR', '3', '1.24246440412', '-2.3096004705']\n",
      "['SENSOR', '7', '4.5411888941', '-1.80078211601']\n",
      "['ODOMETRY', '-0.0104250459457', '0.100075632863', '-0.0188431839508']\n",
      "['SENSOR', '9', '3.48168308597', '-0.705073049043']\n",
      "['SENSOR', '2', '4.51016536991', '-2.85015444599']\n",
      "['SENSOR', '3', '1.17397867689', '-2.26384820849']\n",
      "['SENSOR', '7', '4.58368921745', '-1.81442387473']\n",
      "['ODOMETRY', '-0.0101771473921', '0.100011050735', '-0.0196083731561']\n",
      "['SENSOR', '9', '3.53348326087', '-0.784447831034']\n",
      "['SENSOR', '2', '4.52117731575', '-2.89659106551']\n",
      "['SENSOR', '3', '1.39890586374', '-2.14286560722']\n",
      "['SENSOR', '7', '4.76172397197', '-1.82219242799']\n",
      "['ODOMETRY', '-0.0101785548489', '0.100004838788', '-0.0202689362584']\n",
      "['SENSOR', '9', '3.44733618805', '-0.725368623109']\n",
      "['SENSOR', '2', '4.69146013992', '-2.87115134275']\n",
      "['SENSOR', '3', '1.50765435383', '-2.22245197353']\n",
      "['SENSOR', '7', '4.48642435312', '-1.84876318884']\n",
      "['ODOMETRY', '-0.0091227397028', '0.0999924972858', '-0.0181685651638']\n",
      "['SENSOR', '9', '3.22534891044', '-0.724699872244']\n",
      "['SENSOR', '2', '4.82403976411', '-2.9345124872']\n",
      "['SENSOR', '3', '1.40944143312', '-2.41378091735']\n",
      "['SENSOR', '7', '4.61353587116', '-1.78898785581']\n",
      "['ODOMETRY', '-0.00990767375964', '0.0999963798923', '-0.0204463659192']\n",
      "['SENSOR', '9', '3.2978264557', '-0.848937354757']\n",
      "['SENSOR', '2', '4.79210487087', '-2.88027569823']\n",
      "['SENSOR', '3', '1.57460267827', '-2.35009049783']\n",
      "['SENSOR', '7', '4.86239210585', '-1.68728512464']\n",
      "['ODOMETRY', '-0.0105209157752', '0.100056520755', '-0.0199923529952']\n",
      "['SENSOR', '9', '3.11237333983', '-0.828608726065']\n",
      "['SENSOR', '2', '5.1348537585', '-2.83576534622']\n",
      "['SENSOR', '3', '1.7099726366', '-2.2858612058']\n",
      "['SENSOR', '7', '4.90049538427', '-1.78150085169']\n",
      "['ODOMETRY', '-0.010155344285', '0.100089786643', '-0.0198085659559']\n",
      "['SENSOR', '9', '2.99996691085', '-0.765112595878']\n",
      "['SENSOR', '3', '1.6542766528', '-2.40104297088']\n",
      "['SENSOR', '7', '4.68837972288', '-1.72827448368']\n",
      "['ODOMETRY', '-0.00967291193757', '0.100124175068', '-0.0205587676625']\n",
      "['SENSOR', '9', '2.99807347078', '-0.696237174014']\n",
      "['SENSOR', '3', '1.7267487227', '-2.3662440395']\n",
      "['SENSOR', '7', '4.7350984348', '-1.75139975295']\n",
      "['ODOMETRY', '-0.0101662059319', '0.100228934457', '-0.0199252685348']\n",
      "['SENSOR', '9', '3.15312197773', '-0.769591095276']\n",
      "['SENSOR', '3', '1.73627144755', '-2.31623144182']\n",
      "['SENSOR', '7', '5.06411728014', '-1.71705818396']\n",
      "['ODOMETRY', '-0.0107211355799', '0.0999927335192', '-0.020501983192']\n",
      "['SENSOR', '9', '2.86278392437', '-0.743762595525']\n",
      "['SENSOR', '3', '1.84695799822', '-2.43783547802']\n",
      "['SENSOR', '7', '5.07231532054', '-1.68808003478']\n",
      "['ODOMETRY', '-0.00979459746259', '0.100025076699', '-0.0197251786846']\n",
      "['SENSOR', '9', '2.82075829413', '-0.688148179119']\n",
      "['SENSOR', '3', '2.02116189813', '-2.39856289262']\n",
      "['SENSOR', '7', '4.91405340171', '-1.71247547809']\n",
      "['ODOMETRY', '-0.00880991332931', '0.0997863392491', '-0.0197458769482']\n",
      "['SENSOR', '9', '2.83728718149', '-0.703540952524']\n",
      "['SENSOR', '3', '1.93000650123', '-2.36970627286']\n",
      "['SENSOR', '7', '4.79347993043', '-1.6329273271']\n",
      "['ODOMETRY', '-0.0100580444163', '0.10021748682', '-0.0200095441708']\n",
      "['SENSOR', '9', '2.51641560778', '-0.756300137634']\n",
      "['SENSOR', '3', '2.09607292645', '-2.38526777678']\n",
      "['SENSOR', '7', '5.06835478059', '-1.72475485601']\n",
      "['ODOMETRY', '-0.0104281508158', '0.10000140247', '-0.0199377152457']\n",
      "['SENSOR', '9', '2.45948289837', '-0.76021264418']\n",
      "['SENSOR', '3', '2.06522117309', '-2.33318420312']\n",
      "['SENSOR', '7', '4.84613918824', '-1.61007685337']\n",
      "['ODOMETRY', '-0.0097623943034', '0.0999813928244', '-0.0206369980195']\n",
      "['SENSOR', '9', '2.57479230149', '-0.632819077978']\n",
      "['SENSOR', '3', '2.177346285', '-2.42626146831']\n",
      "['SENSOR', '7', '5.05551333216', '-1.69863156486']\n",
      "['ODOMETRY', '-0.00975220094369', '0.100042185698', '-0.0200047749536']\n",
      "['SENSOR', '9', '2.5074325001', '-0.609410447289']\n",
      "['SENSOR', '3', '2.3010604256', '-2.35179262067']\n",
      "['SENSOR', '7', '4.90749675187', '-1.70419293696']\n",
      "['ODOMETRY', '-0.00927676045743', '0.100208064048', '-0.0191796860605']\n",
      "['SENSOR', '9', '2.41626913018', '-0.724594121384']\n",
      "['SENSOR', '3', '2.52272282794', '-2.4239408799']\n",
      "['SENSOR', '7', '4.70640264651', '-1.63618162589']\n",
      "['ODOMETRY', '-0.0099067860188', '0.0998711486494', '-0.0194012350076']\n",
      "['SENSOR', '9', '2.24529394017', '-0.685502521964']\n",
      "['SENSOR', '3', '2.31844097149', '-2.42828855555']\n",
      "['SENSOR', '7', '4.95528000197', '-1.57288604154']\n",
      "['ODOMETRY', '-0.00983384980744', '0.0999224766995', '-0.0204305528243']\n",
      "['SENSOR', '9', '2.18268166452', '-0.647873105786']\n",
      "['SENSOR', '3', '2.60693896315', '-2.37482011041']\n",
      "['SENSOR', '7', '4.87407767625', '-1.66971167459']\n",
      "['ODOMETRY', '-0.0102628286794', '0.0997605730161', '-0.0210500632473']\n",
      "['SENSOR', '9', '2.15683089899', '-0.642555173461']\n",
      "['SENSOR', '3', '2.6756869293', '-2.29039615081']\n",
      "['SENSOR', '7', '5.11674482285', '-1.47951929626']\n",
      "['ODOMETRY', '-0.0101962211399', '0.099836539792', '-0.0188237479265']\n",
      "['SENSOR', '9', '1.9915555629', '-0.605343778173']\n",
      "['SENSOR', '3', '2.8321255953', '-2.36581198314']\n",
      "['SENSOR', '7', '4.93146123799', '-1.5890166699']\n",
      "['ODOMETRY', '-0.0109072611821', '0.100057984487', '-0.0199246493126']\n",
      "['SENSOR', '9', '2.05060986776', '-0.812359999266']\n",
      "['SENSOR', '3', '2.89469192638', '-2.33538439474']\n",
      "['SENSOR', '7', '5.06379731865', '-1.57967642574']\n",
      "['ODOMETRY', '-0.0110088218391', '0.10014697699', '-0.0202732496012']\n",
      "['SENSOR', '9', '1.9855033654', '-0.729611380785']\n",
      "['SENSOR', '3', '2.808098691', '-2.40456966848']\n",
      "['SENSOR', '7', '5.00506879142', '-1.52174912725']\n",
      "['ODOMETRY', '-0.0097077990525', '0.100157141283', '-0.0214692363299']\n",
      "['SENSOR', '9', '1.85666944851', '-0.732607970484']\n",
      "['SENSOR', '3', '3.15325419611', '-2.35565991968']\n",
      "['SENSOR', '7', '4.77716786006', '-1.61242538926']\n",
      "['ODOMETRY', '-0.00981948245873', '0.0999149565253', '-0.020711936978']\n",
      "['SENSOR', '9', '1.66695280875', '-0.677940267447']\n",
      "['SENSOR', '3', '2.93583551959', '-2.29721883235']\n",
      "['SENSOR', '7', '4.77792363509', '-1.55059855566']\n",
      "['ODOMETRY', '-0.00992896039159', '0.0998189682803', '-0.0204511862369']\n",
      "['SENSOR', '9', '1.71819656468', '-0.707245929411']\n",
      "['SENSOR', '3', '3.02406315637', '-2.3968858507']\n",
      "['SENSOR', '7', '4.90173372979', '-1.65060778383']\n",
      "['ODOMETRY', '-0.00884110158486', '0.100015505449', '-0.0206146468981']\n",
      "['SENSOR', '9', '1.70981171667', '-0.684234093975']\n",
      "['SENSOR', '3', '3.16299858885', '-2.37554434755']\n",
      "['SENSOR', '7', '5.05446186142', '-1.55954853363']\n",
      "['ODOMETRY', '-0.0102024861503', '0.10032005589', '-0.0183984058531']\n",
      "['SENSOR', '9', '1.48285099536', '-0.732863940447']\n",
      "['SENSOR', '3', '3.24066838872', '-2.3472192049']\n",
      "['SENSOR', '7', '5.02189688945', '-1.52629273662']\n",
      "['ODOMETRY', '-0.0095354970248', '0.100025384548', '-0.0207858100942']\n",
      "['SENSOR', '9', '1.46150131465', '-0.767666096771']\n",
      "['SENSOR', '3', '3.22029667183', '-2.36535752613']\n",
      "['SENSOR', '7', '4.84479242128', '-1.46298563412']\n",
      "['ODOMETRY', '-0.00906969760687', '0.099796851015', '-0.0208710090182']\n",
      "['SENSOR', '9', '1.53472735309', '-0.701353506585']\n",
      "['SENSOR', '3', '3.30563763836', '-2.28920291979']\n",
      "['SENSOR', '7', '5.08447465054', '-1.48707316086']\n",
      "['ODOMETRY', '-0.00996926665775', '0.100146565387', '-0.018774822561']\n",
      "['SENSOR', '9', '1.27538476788', '-0.825565964241']\n",
      "['SENSOR', '3', '3.52081304084', '-2.35140411346']\n",
      "['SENSOR', '7', '4.9713536015', '-1.50506099629']\n",
      "['ODOMETRY', '-0.00982937584718', '0.100296415285', '-0.0202122708831']\n",
      "['SENSOR', '9', '1.28245800998', '-0.820894690375']\n",
      "['SENSOR', '3', '3.61786607428', '-2.28348332028']\n",
      "['SENSOR', '7', '4.72683380487', '-1.5245034595']\n",
      "['ODOMETRY', '-0.00967296493409', '0.100095993623', '-0.0208377359394']\n",
      "['SENSOR', '9', '1.22717122688', '-0.834555116798']\n",
      "['SENSOR', '3', '3.46599411335', '-2.26643278842']\n",
      "['SENSOR', '7', '4.77048897684', '-1.61915796579']\n",
      "['ODOMETRY', '-0.00983012240915', '0.100048682167', '-0.0200418409326']\n",
      "['SENSOR', '9', '1.21052101312', '-0.846681880547']\n",
      "['SENSOR', '3', '3.70451197871', '-2.25861912203']\n",
      "['SENSOR', '7', '5.07806532139', '-1.49123903126']\n",
      "['ODOMETRY', '-0.00930456459578', '0.100071988622', '-0.0204347040085']\n",
      "['SENSOR', '9', '1.12120365478', '-0.935390860361']\n",
      "['SENSOR', '3', '3.58620574347', '-2.27331836708']\n",
      "['SENSOR', '7', '4.76861242282', '-1.53629792407']\n",
      "['ODOMETRY', '0.000468699390922', '0.0998304456128', '-2.5849995276e-05']\n",
      "['SENSOR', '9', '1.14054428171', '-0.998313577557']\n",
      "['SENSOR', '3', '3.55052396622', '-2.27149420334']\n",
      "['SENSOR', '6', '4.97342840955', '-0.481153301138']\n",
      "['SENSOR', '7', '4.93140206369', '-1.5097071492']\n",
      "['ODOMETRY', '-0.000306172617703', '0.0998659733612', '0.00088045160042']\n",
      "['SENSOR', '9', '0.908943386111', '-1.05078434189']\n",
      "['SENSOR', '3', '3.92854556221', '-2.37317280336']\n",
      "['SENSOR', '6', '4.65017824828', '-0.48503026696']\n",
      "['SENSOR', '7', '4.82256937785', '-1.46085672931']\n",
      "['ODOMETRY', '6.35865396476e-05', '0.100145996251', '0.000825376841077']\n",
      "['SENSOR', '9', '0.827243169323', '-1.09325181967']\n",
      "['SENSOR', '3', '3.73413074447', '-2.37141305415']\n",
      "['SENSOR', '6', '4.78905396728', '-0.408920738147']\n",
      "['SENSOR', '7', '4.99209306977', '-1.55682191086']\n",
      "['ODOMETRY', '0.000201838205791', '0.100136187552', '-0.00014387330981']\n",
      "['SENSOR', '9', '0.854965120184', '-1.30243719417']\n",
      "['SENSOR', '3', '4.08986785793', '-2.31375803278']\n",
      "['SENSOR', '6', '4.49746631975', '-0.415197960969']\n",
      "['SENSOR', '7', '4.91878504334', '-1.53841931157']\n",
      "['ODOMETRY', '0.000309566514052', '0.099997143531', '0.000424327411615']\n",
      "['SENSOR', '9', '0.829574081247', '-1.34777510094']\n",
      "['SENSOR', '3', '3.99649915462', '-2.37661430213']\n",
      "['SENSOR', '6', '4.86215564276', '-0.354259931521']\n",
      "['SENSOR', '7', '5.17293988297', '-1.61785902826']\n",
      "['ODOMETRY', '-0.000499775865667', '0.0998879891969', '-0.000202950445095']\n",
      "['SENSOR', '9', '0.926234276526', '-1.50863813963']\n",
      "['SENSOR', '3', '4.06561199195', '-2.33294732347']\n",
      "['SENSOR', '6', '4.52518969077', '-0.459458859882']\n",
      "['SENSOR', '7', '4.90713478296', '-1.6534950895']\n",
      "['ODOMETRY', '0.000159821856273', '0.0999413115697', '0.000165823443296']\n",
      "['SENSOR', '9', '0.798977474978', '-1.61450594503']\n",
      "['SENSOR', '3', '4.24005259522', '-2.48339290405']\n",
      "['SENSOR', '6', '4.37753199658', '-0.439993648344']\n",
      "['SENSOR', '7', '4.78729857286', '-1.52285334952']\n",
      "['ODOMETRY', '0.000239336090803', '0.100024339541', '-0.000702745140684']\n",
      "['SENSOR', '9', '0.85153318303', '-1.74081822727']\n",
      "['SENSOR', '3', '4.19445086514', '-2.41078939754']\n",
      "['SENSOR', '6', '4.3332692815', '-0.5738717181']\n",
      "['SENSOR', '7', '4.82886107215', '-1.58958533264']\n",
      "['ODOMETRY', '0.000302502313308', '0.0999098702903', '-0.000677343238']\n",
      "['SENSOR', '9', '1.00720101453', '-1.80629458243']\n",
      "['SENSOR', '3', '4.35581118198', '-2.52872962107']\n",
      "['SENSOR', '6', '4.26273136747', '-0.561343932255']\n",
      "['SENSOR', '7', '4.90576859962', '-1.57561915161']\n",
      "['ODOMETRY', '-0.000113543700968', '0.0999631337801', '5.71158197463e-05']\n",
      "['SENSOR', '9', '0.926859129886', '-2.02052805263']\n",
      "['SENSOR', '3', '4.5099200538', '-2.5031268995']\n",
      "['SENSOR', '6', '4.1118649023', '-0.581633953961']\n",
      "['SENSOR', '7', '4.76898001883', '-1.59231791735']\n",
      "['ODOMETRY', '-0.000339024488948', '0.100009711461', '2.26344713328e-05']\n",
      "['SENSOR', '9', '1.01426369829', '-2.06024600625']\n",
      "['SENSOR', '3', '4.50108503541', '-2.50066382553']\n",
      "['SENSOR', '6', '4.10077035421', '-0.552157899173']\n",
      "['SENSOR', '7', '4.90578246686', '-1.60737195764']\n",
      "['ODOMETRY', '-0.000504457264396', '0.100013657645', '-0.000177576169986']\n",
      "['SENSOR', '9', '0.887211360034', '-2.18963300619']\n",
      "['SENSOR', '3', '4.5749440351', '-2.48258346425']\n",
      "['SENSOR', '6', '3.90635999301', '-0.524342529557']\n",
      "['SENSOR', '7', '4.99010419492', '-1.6897455089']\n",
      "['ODOMETRY', '0.000243302162545', '0.100124792442', '0.000564451310213']\n",
      "['SENSOR', '9', '0.950336814138', '-2.20135217535']\n",
      "['SENSOR', '3', '4.64575251122', '-2.45474381848']\n",
      "['SENSOR', '6', '3.94194574093', '-0.493874197364']\n",
      "['SENSOR', '7', '5.01810231369', '-1.75192836429']\n",
      "['ODOMETRY', '-0.000738312989045', '0.100017614324', '0.000452576665471']\n",
      "['SENSOR', '9', '1.24742332493', '-2.3761257833']\n",
      "['SENSOR', '3', '4.78533958114', '-2.48110189333']\n",
      "['SENSOR', '6', '3.83981394901', '-0.473736803464']\n",
      "['SENSOR', '7', '4.94028734088', '-1.67119507241']\n",
      "['ODOMETRY', '-0.000158281123923', '0.100057519587', '8.25146303648e-05']\n",
      "['SENSOR', '9', '1.25516136996', '-2.38310746911']\n",
      "['SENSOR', '3', '4.67629555186', '-2.57984253808']\n",
      "['SENSOR', '6', '3.58479173705', '-0.552269803889']\n",
      "['SENSOR', '7', '5.04082293108', '-1.86475241589']\n",
      "['ODOMETRY', '-0.000421693045302', '0.100088421092', '-0.000583909844015']\n",
      "['SENSOR', '9', '1.30043780911', '-2.42814444723']\n",
      "['SENSOR', '3', '4.77942694559', '-2.51108430416']\n",
      "['SENSOR', '6', '3.7036104257', '-0.594306761593']\n",
      "['SENSOR', '7', '5.20342066846', '-1.82597751664']\n",
      "['ODOMETRY', '-0.000492703155817', '0.0998767019988', '-0.000399546911055']\n",
      "['SENSOR', '9', '1.50701523022', '-2.40519251821']\n",
      "['SENSOR', '3', '4.97415244053', '-2.57426809268']\n",
      "['SENSOR', '6', '3.73937872599', '-0.646973765524']\n",
      "['SENSOR', '7', '5.04630156868', '-1.81126053621']\n",
      "['ODOMETRY', '-0.000686941708413', '0.100091264802', '-0.000646733220067']\n",
      "['SENSOR', '9', '1.53949734641', '-2.56047560031']\n",
      "['SENSOR', '6', '3.54771183278', '-0.554710714875']\n",
      "['ODOMETRY', '0.000264360795126', '0.0999909746645', '-0.000504895193083']\n",
      "['SENSOR', '9', '1.51238925612', '-2.52278575906']\n",
      "['SENSOR', '6', '3.27990661032', '-0.58279346945']\n",
      "['ODOMETRY', '-1.55641718837e-05', '0.100163521473', '-0.000133745408038']\n",
      "['SENSOR', '9', '1.5098952529', '-2.57185137154']\n",
      "['SENSOR', '6', '3.39256354505', '-0.708652206512']\n",
      "['ODOMETRY', '-0.00944281048405', '0.0998363016919', '-0.0197064530302']\n",
      "['SENSOR', '9', '1.7405212542', '-2.61856151363']\n",
      "['SENSOR', '6', '3.4449354883', '-0.653656313882']\n",
      "['ODOMETRY', '-0.00982171572374', '0.0999283940399', '-0.0201816524328']\n",
      "['SENSOR', '9', '1.65301995925', '-2.56952419519']\n",
      "['SENSOR', '6', '3.02584430907', '-0.625348664803']\n",
      "['ODOMETRY', '-0.011467407889', '0.100278553593', '-0.0200899025046']\n",
      "['SENSOR', '9', '1.84416645507', '-2.56385985305']\n",
      "['SENSOR', '6', '3.02960198478', '-0.517486552302']\n",
      "['ODOMETRY', '-0.00949998918607', '0.100016536484', '-0.0192452653175']\n",
      "['SENSOR', '9', '1.83946050295', '-2.63003003289']\n",
      "['SENSOR', '6', '3.00582309666', '-0.577468671279']\n",
      "['ODOMETRY', '-0.00998770181841', '0.100041220527', '-0.0210200884984']\n",
      "['SENSOR', '9', '2.1231956515', '-2.5712329806']\n",
      "['SENSOR', '6', '2.72060090136', '-0.611342981081']\n",
      "['ODOMETRY', '-0.0104435233728', '0.100064843828', '-0.0209923840156']\n",
      "['SENSOR', '9', '2.2533253491', '-2.51358315181']\n",
      "['SENSOR', '6', '2.76502567229', '-0.610392844017']\n",
      "['ODOMETRY', '-0.0101773588174', '0.100165429114', '-0.0197769867168']\n",
      "['SENSOR', '9', '2.27395359652', '-2.62138399446']\n",
      "['SENSOR', '6', '2.81928646279', '-0.499702125893']\n",
      "['ODOMETRY', '-0.00963806206216', '0.100194140395', '-0.0202377586004']\n",
      "['SENSOR', '9', '2.19331599608', '-2.58825731994']\n",
      "['SENSOR', '6', '2.6367644419', '-0.585514977039']\n",
      "['ODOMETRY', '-0.00969850517913', '0.100084649032', '-0.0200844906424']\n",
      "['SENSOR', '9', '2.38239873462', '-2.51826314964']\n",
      "['SENSOR', '6', '2.55584502849', '-0.617585417568']\n",
      "['ODOMETRY', '-0.009760488778', '0.100025282561', '-0.0212037030791']\n",
      "['SENSOR', '9', '2.55229313835', '-2.55940451124']\n",
      "['SENSOR', '6', '2.63729703758', '-0.564546146269']\n",
      "['ODOMETRY', '-0.00901024455226', '0.0999822225335', '-0.0203196475783']\n",
      "['SENSOR', '9', '2.542229302', '-2.53541280471']\n",
      "['SENSOR', '6', '2.27227869215', '-0.573875753025']\n",
      "['ODOMETRY', '-0.00961564138041', '0.0999414405798', '-0.0204022645065']\n",
      "['SENSOR', '9', '2.70341312756', '-2.48331655508']\n",
      "['SENSOR', '6', '2.28488250206', '-0.552514217678']\n",
      "['ODOMETRY', '-0.00990457116932', '0.100026465638', '-0.0207767178837']\n",
      "['SENSOR', '9', '2.56436735907', '-2.50931464148']\n",
      "['SENSOR', '6', '2.08017880027', '-0.487502087503']\n",
      "['ODOMETRY', '-0.00998320897741', '0.0999813680912', '-0.0199150821834']\n",
      "['SENSOR', '9', '2.658209228', '-2.52967300837']\n",
      "['SENSOR', '6', '2.08742861878', '-0.468280843216']\n",
      "['ODOMETRY', '-0.0107252267638', '0.10021159563', '-0.0192940861751']\n",
      "['SENSOR', '9', '2.79062572373', '-2.51801590567']\n",
      "['SENSOR', '6', '1.98134535694', '-0.51239911617']\n",
      "['ODOMETRY', '-0.00931898482752', '0.0998313946401', '-0.0195045943639']\n",
      "['SENSOR', '9', '2.97631712843', '-2.59124598555']\n",
      "['SENSOR', '6', '1.83278194557', '-0.45058413267']\n",
      "['ODOMETRY', '-0.0102760318389', '0.0999106615633', '-0.0196442889398']\n",
      "['SENSOR', '9', '3.00538927132', '-2.47582875838']\n",
      "['SENSOR', '5', '4.86844686309', '-0.644513040378']\n",
      "['SENSOR', '6', '1.75818612283', '-0.557237052247']\n",
      "['ODOMETRY', '-0.0104218597473', '0.100145397313', '-0.0200408029886']\n",
      "['SENSOR', '9', '3.06790211886', '-2.40968598481']\n",
      "['SENSOR', '5', '4.81572738233', '-0.616401204908']\n",
      "['SENSOR', '6', '1.68748831042', '-0.389912308279']\n",
      "['ODOMETRY', '-0.0101350745755', '0.100091285908', '-0.0206119427088']\n",
      "['SENSOR', '9', '3.05939885103', '-2.42259296594']\n",
      "['SENSOR', '5', '4.85625275968', '-0.554387962083']\n",
      "['SENSOR', '6', '1.69976007626', '-0.4523568891']\n",
      "['ODOMETRY', '-0.0112125747693', '0.100293480859', '-0.0200290197221']\n",
      "['SENSOR', '9', '3.36060085358', '-2.4353356474']\n",
      "['SENSOR', '5', '4.60467409396', '-0.689671837769']\n",
      "['SENSOR', '6', '1.66466459286', '-0.434947234979']\n",
      "['ODOMETRY', '-0.00936003709598', '0.0997788840301', '-0.0195655025372']\n",
      "['SENSOR', '9', '3.24434042861', '-2.50085985671']\n",
      "['SENSOR', '5', '4.44097432373', '-0.539545027585']\n",
      "['SENSOR', '6', '1.44534873942', '-0.340798495725']\n",
      "['ODOMETRY', '-0.0105609095644', '0.100090264715', '-0.0197529740061']\n",
      "['SENSOR', '9', '3.43181578801', '-2.38831289505']\n",
      "['SENSOR', '5', '4.47137535155', '-0.631269209482']\n",
      "['SENSOR', '6', '1.26851574319', '-0.398939844547']\n",
      "['ODOMETRY', '-0.0107658587575', '0.0999709096433', '-0.0195658499488']\n",
      "['SENSOR', '9', '3.50551920675', '-2.39732140232']\n",
      "['SENSOR', '5', '4.28281556549', '-0.589616154158']\n",
      "['SENSOR', '6', '1.28831954512', '-0.513359363001']\n",
      "['ODOMETRY', '-0.0101758354185', '0.10002526256', '-0.0193481524309']\n",
      "['SENSOR', '9', '3.67081338849', '-2.3789885528']\n",
      "['SENSOR', '5', '4.40168128717', '-0.489968693872']\n",
      "['SENSOR', '6', '1.19631205076', '-0.512575604526']\n",
      "['ODOMETRY', '-0.0110076328184', '0.100028655375', '-0.0196243037287']\n",
      "['SENSOR', '9', '3.62146176736', '-2.30271042095']\n",
      "['SENSOR', '5', '4.18233806232', '-0.556833847024']\n",
      "['SENSOR', '6', '1.01671369638', '-0.459323796962']\n",
      "['ODOMETRY', '-0.00955580720005', '0.0998135945322', '-0.0197517141062']\n",
      "['SENSOR', '9', '3.70417031979', '-2.34473876598']\n",
      "['SENSOR', '5', '4.17062919093', '-0.570876489648']\n",
      "['SENSOR', '6', '0.923913647873', '-0.520206587019']\n",
      "['ODOMETRY', '-0.0091535851559', '0.100020863652', '-0.0201880877397']\n",
      "['SENSOR', '9', '3.69578950794', '-2.41413549551']\n",
      "['SENSOR', '5', '4.00110518645', '-0.494969326567']\n",
      "['SENSOR', '6', '0.964762619449', '-0.53458348217']\n",
      "['ODOMETRY', '-0.010295675802', '0.100004015507', '-0.0189695088639']\n",
      "['SENSOR', '9', '3.69844459713', '-2.31420630353']\n",
      "['SENSOR', '5', '3.93813832417', '-0.404459278547']\n",
      "['SENSOR', '6', '0.762339598312', '-0.528477333035']\n",
      "['ODOMETRY', '-0.00986627304745', '0.0999308954255', '-0.0194932169016']\n",
      "['SENSOR', '9', '3.97863915035', '-2.33817010298']\n",
      "['SENSOR', '5', '3.91815869731', '-0.375382277621']\n",
      "['SENSOR', '6', '0.764457865234', '-0.692422479677']\n",
      "['ODOMETRY', '-0.0105039330611', '0.100062635978', '-0.0196007510172']\n",
      "['SENSOR', '9', '3.99049095935', '-2.43100832721']\n",
      "['SENSOR', '5', '3.84981410268', '-0.339858112469']\n",
      "['SENSOR', '6', '0.670707796904', '-0.627865459926']\n",
      "['ODOMETRY', '-0.0102670061231', '0.100220134545', '-0.0197649412615']\n",
      "['SENSOR', '9', '3.97444895632', '-2.34925609157']\n",
      "['SENSOR', '5', '3.77657198716', '-0.365748954501']\n",
      "['SENSOR', '6', '0.724620745167', '-0.68784406447']\n",
      "['ODOMETRY', '-0.00978517807132', '0.0997958111892', '-0.0209122965862']\n",
      "['SENSOR', '9', '4.03143217141', '-2.26626563102']\n",
      "['SENSOR', '5', '3.46349133261', '-0.34264365269']\n",
      "['SENSOR', '6', '0.475188697577', '-0.867727704814']\n",
      "['ODOMETRY', '-0.0108972815108', '0.0998936621075', '-0.0198820412631']\n",
      "['SENSOR', '9', '4.23985887378', '-2.26854947811']\n",
      "['SENSOR', '5', '3.41389965628', '-0.282744929273']\n",
      "['SENSOR', '6', '0.627709996436', '-0.94148755275']\n",
      "['ODOMETRY', '-0.0092026691367', '0.0998946022284', '-0.0202789115158']\n",
      "['SENSOR', '9', '4.11518464247', '-2.31925173137']\n",
      "['SENSOR', '5', '3.37747667925', '-0.382887053434']\n",
      "['SENSOR', '6', '0.347854511055', '-1.06521018247']\n",
      "['ODOMETRY', '-0.00964561978411', '0.0999128007624', '-0.0203232411783']\n",
      "['SENSOR', '9', '4.2592528018', '-2.25631821864']\n",
      "['SENSOR', '5', '3.48949092568', '-0.316251464921']\n",
      "['SENSOR', '6', '0.418415784686', '-1.24819855562']\n",
      "['ODOMETRY', '-0.0090889765955', '0.100078317535', '-0.0208387649645']\n",
      "['SENSOR', '9', '4.38985383683', '-2.35866286668']\n",
      "['SENSOR', '5', '3.28107289655', '-0.276801161352']\n",
      "['SENSOR', '6', '0.520377085458', '-1.46790593847']\n",
      "['ODOMETRY', '-0.00947836960967', '0.0999378658775', '-0.0205443341273']\n",
      "['SENSOR', '9', '4.49482605041', '-2.26889815025']\n",
      "['SENSOR', '5', '3.15258240035', '-0.336438423951']\n",
      "['SENSOR', '6', '0.430980850097', '-1.70329062466']\n",
      "['ODOMETRY', '-0.00935420041505', '0.100110622918', '-0.0200634654475']\n",
      "['SENSOR', '9', '4.56500399918', '-2.3031466037']\n",
      "['SENSOR', '5', '2.91571056826', '-0.232623268414']\n",
      "['SENSOR', '6', '0.330244890906', '-1.90953716454']\n",
      "['ODOMETRY', '-0.010269880911', '0.100061171307', '-0.0197644075107']\n",
      "['SENSOR', '9', '4.69167462854', '-2.22658998309']\n",
      "['SENSOR', '5', '3.08433569089', '-0.337143319208']\n",
      "['SENSOR', '6', '0.554762750925', '-2.10973524603']\n",
      "['ODOMETRY', '-0.00999199793928', '0.0999665305154', '-0.0202757591957']\n",
      "['SENSOR', '9', '4.76284132902', '-2.15126005988']\n",
      "['SENSOR', '5', '2.95938070311', '-0.158178168659']\n",
      "['SENSOR', '6', '0.549503125028', '-2.27808301514']\n",
      "['ODOMETRY', '-0.00986005097223', '0.100173854445', '-0.0189859725838']\n",
      "['SENSOR', '9', '4.62199069994', '-2.18236767895']\n",
      "['SENSOR', '5', '2.88364841405', '-0.211583718625']\n",
      "['SENSOR', '6', '0.608371630443', '-2.36582694156']\n",
      "['ODOMETRY', '-0.0101721448458', '0.0998079193853', '-0.0198301989047']\n",
      "['SENSOR', '9', '4.6158486866', '-2.14373953498']\n",
      "['SENSOR', '5', '2.58243352082', '-0.200858606282']\n",
      "['SENSOR', '6', '0.747480535627', '-2.41041090005']\n",
      "['ODOMETRY', '-0.0103893957533', '0.0999764229641', '-0.0201432882674']\n",
      "['SENSOR', '9', '4.86345125952', '-2.16850372828']\n",
      "['SENSOR', '5', '2.53715647378', '-0.0219189488566']\n",
      "['SENSOR', '6', '0.638025009019', '-2.44082398988']\n",
      "['ODOMETRY', '-0.0110796247981', '0.100123354178', '-0.0203009558875']\n",
      "['SENSOR', '9', '4.80447773758', '-2.11401632864']\n",
      "['SENSOR', '5', '2.52598129404', '-0.147975261228']\n",
      "['SENSOR', '6', '0.774158398614', '-2.52392662998']\n",
      "['ODOMETRY', '-0.0105396114623', '0.100008052618', '-0.0201317969487']\n",
      "['SENSOR', '9', '4.92716774584', '-2.183105981']\n",
      "['SENSOR', '5', '2.5239692479', '-0.10125627604']\n",
      "['SENSOR', '6', '0.898459257994', '-2.5942905006']\n",
      "['ODOMETRY', '-0.0101753448007', '0.100002913472', '-0.0206574320107']\n",
      "['SENSOR', '9', '5.01194140521', '-2.08598842578']\n",
      "['SENSOR', '5', '2.42112473613', '-0.0714350873771']\n",
      "['SENSOR', '6', '0.878381860102', '-2.66964579914']\n",
      "['ODOMETRY', '-0.0103462997287', '0.0999423849927', '-0.0195755267881']\n",
      "['SENSOR', '5', '2.23964032564', '-0.0586867747141']\n",
      "['SENSOR', '6', '1.24988717446', '-2.58043089475']\n",
      "['ODOMETRY', '-0.0107865547639', '0.10003293752', '-0.0196069887085']\n",
      "['SENSOR', '5', '1.94979000746', '-0.0453900849002']\n",
      "['SENSOR', '6', '1.290168152', '-2.6828250479']\n",
      "['ODOMETRY', '-0.0108544957486', '0.100230199128', '-0.0189367396772']\n",
      "['SENSOR', '5', '2.22911731184', '-0.00122688508787']\n",
      "['SENSOR', '6', '1.21205806069', '-2.64632589698']\n",
      "['ODOMETRY', '-0.00993341143727', '0.0999418232338', '-0.0202662925326']\n",
      "['SENSOR', '4', '4.99272912579', '-0.274166164371']\n",
      "['SENSOR', '5', '1.81092330137', '0.0761110639692']\n",
      "['SENSOR', '6', '1.36882473059', '-2.60089763233']\n",
      "['ODOMETRY', '-0.0104826202942', '0.0999751788151', '-0.0191967893743']\n",
      "['SENSOR', '4', '4.69854535708', '-0.223485928377']\n",
      "['SENSOR', '5', '1.7001461128', '0.0429298228702']\n",
      "['SENSOR', '6', '1.30623051674', '-2.58399330818']\n",
      "['ODOMETRY', '0.000324810885789', '0.0998821776257', '-0.000359989641411']\n",
      "['SENSOR', '4', '5.09005860279', '-0.233743736727']\n",
      "['SENSOR', '5', '1.58907104446', '0.12166808225']\n",
      "['SENSOR', '6', '1.50018263441', '-2.63967144455']\n",
      "['ODOMETRY', '-0.000418164888098', '0.0999656435967', '-3.68260681419e-05']\n",
      "['SENSOR', '4', '4.74946984204', '-0.126245943621']\n",
      "['SENSOR', '5', '1.55519072464', '0.0805634344737']\n",
      "['SENSOR', '6', '1.63437464886', '-2.74620564163']\n",
      "['ODOMETRY', '0.000475115856201', '0.100024313407', '0.000393598981251']\n",
      "['SENSOR', '4', '4.50322759327', '-0.208257404441']\n",
      "['SENSOR', '5', '1.40431609203', '0.102953443634']\n",
      "['SENSOR', '6', '1.92474673868', '-2.70532942045']\n",
      "['SENSOR', '7', '4.8035115893', '-1.34951568347']\n",
      "['ODOMETRY', '-8.36142030851e-05', '0.0998721448335', '-0.000260661559938']\n",
      "['SENSOR', '4', '4.36260790638', '-0.247754318384']\n",
      "['SENSOR', '5', '1.47019886492', '0.0682113654816']\n",
      "['SENSOR', '6', '1.77940808395', '-2.71217766237']\n",
      "['SENSOR', '7', '5.2180857839', '-1.31953374687']\n",
      "['ODOMETRY', '0.000197959165217', '0.100131767346', '0.000281610671693']\n",
      "['SENSOR', '4', '4.44601043546', '-0.326392394425']\n",
      "['SENSOR', '5', '1.19003310152', '0.115920050334']\n",
      "['SENSOR', '6', '2.05288544044', '-2.83359564739']\n",
      "['SENSOR', '7', '4.97368047041', '-1.40535275082']\n",
      "['ODOMETRY', '-0.000164122759813', '0.0999543664159', '-0.000303793093368']\n",
      "['SENSOR', '4', '4.42845729357', '-0.199738979168']\n",
      "['SENSOR', '5', '1.17917386982', '0.18688342145']\n",
      "['SENSOR', '6', '1.9596965856', '-2.82416076788']\n",
      "['SENSOR', '7', '4.92694129534', '-1.35478197562']\n",
      "['ODOMETRY', '0.000333398378648', '0.0999246910299', '-0.000238132530679']\n",
      "['SENSOR', '4', '4.10993462238', '-0.218612334131']\n",
      "['SENSOR', '5', '1.02261696425', '0.152968841424']\n",
      "['SENSOR', '6', '2.17175440342', '-2.77569049944']\n",
      "['SENSOR', '7', '4.9569182734', '-1.483064888']\n",
      "['ODOMETRY', '0.000655493141672', '0.100066766391', '0.000616803666521']\n",
      "['SENSOR', '4', '3.89656801687', '-0.154099643488']\n",
      "['SENSOR', '5', '0.905689372217', '0.172402059922']\n",
      "['SENSOR', '6', '2.13167650164', '-2.75783273628']\n",
      "['SENSOR', '7', '4.8549234685', '-1.39334612816']\n",
      "['ODOMETRY', '-0.000159452291233', '0.100045036471', '0.000808106642051']\n",
      "['SENSOR', '4', '3.94173958574', '-0.302887305467']\n",
      "['SENSOR', '5', '1.14270798286', '0.168760719018']\n",
      "['SENSOR', '6', '2.18631073434', '-2.81520889899']\n",
      "['SENSOR', '7', '5.10636542466', '-1.4209085397']\n",
      "['ODOMETRY', '0.00078211331779', '0.10000393139', '0.000293823867121']\n",
      "['SENSOR', '4', '3.90151568357', '-0.230804046777']\n",
      "['SENSOR', '5', '0.712985585874', '0.132988889564']\n",
      "['SENSOR', '6', '2.15622970424', '-2.85121206477']\n",
      "['SENSOR', '7', '4.91179352895', '-1.43675629418']\n",
      "['ODOMETRY', '-0.000245429391129', '0.0999070378738', '4.52120481994e-05']\n",
      "['SENSOR', '4', '3.64288246527', '-0.35019983237']\n",
      "['SENSOR', '5', '0.799736995544', '0.23802034997']\n",
      "['SENSOR', '6', '2.48255730306', '-2.88618660948']\n",
      "['SENSOR', '7', '4.91606982523', '-1.51036160945']\n",
      "['ODOMETRY', '-0.000170601987707', '0.0999805604304', '-7.40939785008e-05']\n",
      "['SENSOR', '4', '3.60050259878', '-0.29206119154']\n",
      "['SENSOR', '5', '0.456888182182', '0.189816312354']\n",
      "['SENSOR', '6', '2.6445990428', '-2.85742387613']\n",
      "['SENSOR', '7', '4.75304634682', '-1.42550073299']\n",
      "['ODOMETRY', '-0.00152630081795', '0.0999133992614', '-0.000430283518585']\n",
      "['SENSOR', '4', '3.606832886', '-0.248641284229']\n",
      "['SENSOR', '5', '0.495940573578', '0.31820180197']\n",
      "['SENSOR', '6', '2.62200407047', '-2.87985859301']\n",
      "['SENSOR', '7', '4.75456323488', '-1.62447424499']\n",
      "['ODOMETRY', '-0.000217759654145', '0.100001221848', '-2.15306964727e-05']\n",
      "['SENSOR', '4', '3.59697409395', '-0.279183028245']\n",
      "['SENSOR', '5', '0.480705021357', '0.34438043649']\n",
      "['SENSOR', '6', '2.63552269525', '-2.87953212119']\n",
      "['SENSOR', '7', '4.92559449029', '-1.53430624815']\n",
      "['ODOMETRY', '-0.000482506428273', '0.0999122174435', '-0.000580035297792']\n",
      "['SENSOR', '4', '3.47586551117', '-0.349238295616']\n",
      "['SENSOR', '5', '0.220999951716', '0.388622010704']\n",
      "['SENSOR', '6', '2.82004661589', '-2.92802181092']\n",
      "['SENSOR', '7', '4.82070833488', '-1.58213543982']\n",
      "['ODOMETRY', '-0.000139550189593', '0.100064120469', '-2.76254777159e-05']\n",
      "['SENSOR', '4', '3.36308480601', '-0.338986772029']\n",
      "['SENSOR', '5', '0.444200909715', '0.716185023584']\n",
      "['SENSOR', '6', '2.87114105271', '-2.87495975842']\n",
      "['SENSOR', '7', '5.00458709072', '-1.58257275001']\n",
      "['ODOMETRY', '0.000625023111235', '0.100033704567', '0.000788974348719']\n",
      "['SENSOR', '4', '3.13679295783', '-0.355723706289']\n",
      "['SENSOR', '5', '0.132273645824', '1.00432805138']\n",
      "['SENSOR', '6', '3.04078654628', '-2.86651045039']\n",
      "['SENSOR', '7', '4.91504091129', '-1.62691182927']\n",
      "['ODOMETRY', '-0.000430603052429', '0.100031372434', '-0.000194224656002']\n",
      "['SENSOR', '4', '2.89257924978', '-0.261154689677']\n",
      "['SENSOR', '5', '0.0869154821736', '1.57743143981']\n",
      "['SENSOR', '6', '2.90779553154', '-2.87205172826']\n",
      "['SENSOR', '7', '4.87504647749', '-1.67682352072']\n",
      "['ODOMETRY', '-0.000394366473818', '0.0999652691747', '0.000203739446394']\n",
      "['SENSOR', '4', '2.95060694926', '-0.40938842696']\n",
      "['SENSOR', '5', '0.187834365445', '2.26857479846']\n",
      "['SENSOR', '6', '3.31126955451', '-2.92453711135']\n",
      "['SENSOR', '7', '5.00762306838', '-1.62193179913']\n",
      "['ODOMETRY', '0.000188872633703', '0.0998900920204', '-0.000404695062714']\n",
      "['SENSOR', '4', '2.71804063576', '-0.263896340835']\n",
      "['SENSOR', '5', '0.117592609818', '2.55977634237']\n",
      "['SENSOR', '6', '3.49353272518', '-2.94938522616']\n",
      "['SENSOR', '7', '4.88923424474', '-1.71729987801']\n",
      "['ODOMETRY', '0.000463123137535', '0.100026190262', '-0.000467923097136']\n",
      "['SENSOR', '4', '2.79334888537', '-0.399856478188']\n",
      "['SENSOR', '5', '0.493078840954', '2.63845120424']\n",
      "['SENSOR', '6', '3.4746419933', '-2.96022159027']\n",
      "['SENSOR', '7', '4.852250925', '-1.62247582516']\n",
      "['ODOMETRY', '0.000204722786588', '0.100025452409', '-0.000621143247794']\n",
      "['SENSOR', '4', '2.73084876629', '-0.369067978649']\n",
      "['SENSOR', '5', '0.42617115042', '2.86523242658']\n",
      "['SENSOR', '6', '3.81253344685', '-2.96532036628']\n",
      "['SENSOR', '7', '4.96857297672', '-1.68391105412']\n",
      "['ODOMETRY', '-0.000217736658741', '0.0999861864311', '-0.000591307921892']\n",
      "['SENSOR', '4', '2.6677545575', '-0.3747009033']\n",
      "['SENSOR', '5', '0.7159982817', '2.86732646754']\n",
      "['SENSOR', '6', '3.60775134837', '-3.03170946031']\n",
      "['SENSOR', '7', '4.85926199224', '-1.74229396012']\n",
      "['ODOMETRY', '-0.000184559437448', '0.100062080709', '-0.000284379235048']\n",
      "['SENSOR', '4', '2.47524626759', '-0.497455287779']\n",
      "['SENSOR', '5', '0.515976055959', '2.86747266043']\n",
      "['SENSOR', '6', '3.71140327246', '-2.89739069321']\n",
      "['SENSOR', '7', '4.86833979771', '-1.86747905373']\n",
      "['ODOMETRY', '-0.000423831010905', '0.100055729458', '0.000522719071377']\n",
      "['SENSOR', '4', '2.40614223049', '-0.363441019406']\n",
      "['SENSOR', '5', '0.586364241415', '2.87684153835']\n",
      "['SENSOR', '6', '3.90284744099', '-2.93101327542']\n",
      "['SENSOR', '7', '4.77611939003', '-1.8279190371']\n",
      "['ODOMETRY', '-0.000503367053185', '0.100103555799', '0.000819633507756']\n",
      "['SENSOR', '4', '2.40160042935', '-0.389154949411']\n",
      "['SENSOR', '5', '0.853827928964', '3.00466868631']\n",
      "['SENSOR', '6', '3.82496452269', '-2.95364332599']\n",
      "['SENSOR', '7', '5.02564796909', '-1.78367151428']\n",
      "['ODOMETRY', '-0.00080944386785', '0.100013043879', '1.97022648298e-05']\n",
      "['SENSOR', '4', '2.14265941231', '-0.576114496391']\n",
      "['SENSOR', '5', '1.06875104344', '3.01158775394']\n",
      "['SENSOR', '6', '4.04413273461', '-2.9178294235']\n",
      "['SENSOR', '7', '5.0221824592', '-1.79251846775']\n",
      "['ODOMETRY', '-0.000993208621031', '0.0998844685408', '-0.000121644357505']\n",
      "['SENSOR', '4', '2.27760317493', '-0.454019833416']\n",
      "['SENSOR', '5', '0.989955134561', '3.00334167163']\n",
      "['SENSOR', '6', '4.0746975399', '-2.97885744051']\n",
      "['ODOMETRY', '-0.000250538104947', '0.100118558009', '0.000470538017319']\n",
      "['SENSOR', '8', '5.08620163194', '-1.49046025052']\n",
      "['SENSOR', '4', '2.30195764953', '-0.428336448448']\n",
      "['SENSOR', '5', '1.20549687351', '2.9880742534']\n",
      "['SENSOR', '6', '4.35196037607', '-2.99771407728']\n",
      "['ODOMETRY', '-0.000419971039834', '0.0999850157042', '-0.000136873067745']\n",
      "['SENSOR', '8', '4.9606373049', '-1.48374082501']\n",
      "['SENSOR', '4', '2.02122842241', '-0.612520417005']\n",
      "['SENSOR', '5', '1.18548266277', '2.91183826645']\n",
      "['SENSOR', '6', '4.40601584512', '-3.00750175779']\n",
      "['ODOMETRY', '0.000258908008153', '0.0999880869982', '-0.000878618195419']\n",
      "['SENSOR', '8', '5.07928480298', '-1.47517883113']\n",
      "['SENSOR', '4', '1.95540196161', '-0.524791774647']\n",
      "['SENSOR', '5', '1.50601099601', '3.02951420464']\n",
      "['SENSOR', '6', '4.2697515621', '-2.91223271519']\n",
      "['ODOMETRY', '8.35160305286e-05', '0.0998921936279', '-0.000916685935868']\n",
      "['SENSOR', '8', '5.12480337929', '-1.5292251446']\n",
      "['SENSOR', '4', '1.8502364216', '-0.640053414064']\n",
      "['SENSOR', '5', '1.22548414583', '3.00267777115']\n",
      "['SENSOR', '6', '4.59119051094', '-3.00286235802']\n",
      "['ODOMETRY', '0.000276055660063', '0.0999750533485', '0.000328587305689']\n",
      "['SENSOR', '8', '4.85707120873', '-1.55643745133']\n",
      "['SENSOR', '4', '1.6853612932', '-0.640165672521']\n",
      "['SENSOR', '5', '1.40741969379', '3.08301703619']\n",
      "['SENSOR', '6', '4.43722471141', '-2.92421694343']\n",
      "['ODOMETRY', '-0.000612253935663', '0.100030803164', '0.000384082346755']\n",
      "['SENSOR', '8', '4.8148264003', '-1.5861711641']\n",
      "['SENSOR', '4', '1.62945141541', '-0.645906242858']\n",
      "['SENSOR', '5', '1.57411900279', '3.00136290051']\n",
      "['SENSOR', '6', '4.74165161304', '-2.88381335457']\n",
      "['ODOMETRY', '-0.000615965348572', '0.0999645830188', '0.000122066793836']\n",
      "['SENSOR', '8', '5.09246076865', '-1.62935789257']\n",
      "['SENSOR', '4', '1.50283498391', '-0.764000413422']\n",
      "['SENSOR', '5', '1.65903158666', '3.20124071317']\n",
      "['SENSOR', '6', '4.68619938405', '-3.0067714176']\n",
      "['ODOMETRY', '0.00010181891152', '0.100046833019', '0.000440816980011']\n",
      "['SENSOR', '8', '5.10307136294', '-1.5981894044']\n",
      "['SENSOR', '4', '1.3396692961', '-0.647480763035']\n",
      "['SENSOR', '5', '1.89463338292', '3.13168562413']\n",
      "['SENSOR', '6', '4.79308799565', '-3.00728766374']\n",
      "['ODOMETRY', '-0.000291816369108', '0.100048609814', '-0.00104644833934']\n",
      "['SENSOR', '8', '5.05308894221', '-1.61739005656']\n",
      "['SENSOR', '4', '1.55728242344', '-0.778699498829']\n",
      "['SENSOR', '5', '1.76096780816', '3.0522097073']\n",
      "['ODOMETRY', '8.52183257962e-05', '0.0998196908854', '0.000109532441973']\n",
      "['SENSOR', '8', '4.93860295592', '-1.59049668067']\n",
      "['SENSOR', '4', '1.44172934018', '-0.934292188086']\n",
      "['SENSOR', '5', '1.92234124612', '3.05518288622']\n",
      "['ODOMETRY', '7.22338246953e-05', '0.0999023454001', '-3.77129042551e-05']\n",
      "['SENSOR', '8', '5.1479288931', '-1.65822977182']\n",
      "['SENSOR', '4', '1.25455156912', '-0.879623084747']\n",
      "['SENSOR', '5', '2.37861473612', '3.10537197028']\n",
      "['ODOMETRY', '0.000670127696266', '0.100006079133', '0.000625609916127']\n",
      "['SENSOR', '8', '5.09480518246', '-1.65212699893']\n",
      "['SENSOR', '4', '1.40390131939', '-0.879816536972']\n",
      "['SENSOR', '5', '2.20542921923', '3.11477051951']\n",
      "['ODOMETRY', '0.000570783669274', '0.0999546481828', '0.000310239449538']\n",
      "['SENSOR', '4', '1.06631403361', '-1.08193342846']\n",
      "['SENSOR', '5', '2.47203994196', '3.13761545266']\n",
      "['ODOMETRY', '3.85114033074e-05', '0.100176546577', '0.000173947982474']\n",
      "['SENSOR', '4', '1.20265396615', '-1.09036457216']\n",
      "['SENSOR', '5', '2.34144827576', '3.04411849412']\n",
      "['ODOMETRY', '0.000776427377761', '0.099952687123', '-0.000941767872499']\n",
      "['SENSOR', '4', '1.16717333388', '-1.12821635833']\n",
      "['SENSOR', '5', '2.46018754147', '3.12942300941']\n",
      "['ODOMETRY', '-0.000182525437042', '0.0999468132926', '-0.000158790994297']\n",
      "['SENSOR', '4', '1.0123884597', '-1.2839270903']\n",
      "['SENSOR', '5', '2.62303723473', '2.94037324821']\n",
      "['ODOMETRY', '-0.000682345311597', '0.099958072456', '0.00043862067399']\n",
      "['SENSOR', '4', '1.00020435353', '-1.2311017102']\n",
      "['SENSOR', '5', '2.83365938044', '3.10650495611']\n",
      "['ODOMETRY', '-9.19632135689e-07', '0.10002313352', '-0.000224880476229']\n",
      "['SENSOR', '4', '0.849352538956', '-1.46997822321']\n",
      "['SENSOR', '5', '2.85184515953', '3.11408226771']\n",
      "['ODOMETRY', '-0.000798214396223', '0.0999079224924', '0.000213586041509']\n",
      "['SENSOR', '4', '1.10312028502', '-1.48363601494']\n",
      "['SENSOR', '5', '3.00416166275', '3.10884116279']\n",
      "['ODOMETRY', '-0.000984233781833', '0.0999672948533', '7.64590930276e-05']\n",
      "['SENSOR', '4', '1.04848307593', '-1.64994919996']\n",
      "['SENSOR', '5', '3.02744738541', '3.06646084819']\n",
      "['ODOMETRY', '-0.000260628905805', '0.0999789088987', '0.00116057007669']\n",
      "['SENSOR', '4', '1.01403868185', '-1.8074118964']\n",
      "['SENSOR', '5', '3.08384570669', '3.04030179903']\n",
      "['ODOMETRY', '-0.000220095556118', '0.0999551962105', '-0.000481994451249']\n",
      "['SENSOR', '4', '0.949362251022', '-1.93475998957']\n",
      "['SENSOR', '5', '3.02874489038', '3.02677668221']\n",
      "['ODOMETRY', '0.00031060131166', '0.100181543701', '-0.20019265501']\n",
      "['SENSOR', '4', '1.03535256961', '-1.66182916184']\n",
      "['SENSOR', '5', '3.32942750619', '-2.89908305281']\n",
      "['ODOMETRY', '-6.00078729292e-05', '0.100231170262', '-0.197349468283']\n",
      "['SENSOR', '4', '1.23205472755', '-1.61328765107']\n",
      "['SENSOR', '5', '3.58883871764', '-2.79036998463']\n",
      "['ODOMETRY', '0.000778944546602', '0.0997443640523', '-0.196805293229']\n",
      "['SENSOR', '4', '0.980807526601', '-1.37607808602']\n",
      "['SENSOR', '5', '3.44799435666', '-2.62665666687']\n",
      "['ODOMETRY', '-0.000368902911915', '0.100159655151', '-0.19959086214']\n",
      "['SENSOR', '4', '1.30954404554', '-1.43176636885']\n",
      "['SENSOR', '5', '3.51128233361', '-2.49583044536']\n",
      "['ODOMETRY', '0.000875408598912', '0.10049913959', '-0.198612822628']\n",
      "['SENSOR', '4', '0.953865121238', '-1.2816545328']\n",
      "['SENSOR', '5', '3.75050538274', '-2.21025810438']\n",
      "['ODOMETRY', '0.000643301288042', '0.0998677942291', '-0.202643646401']\n",
      "['SENSOR', '4', '0.937678468196', '-1.17091065111']\n",
      "['SENSOR', '5', '3.86489395394', '-1.98135942803']\n",
      "['ODOMETRY', '-0.000515670443917', '0.100048418713', '-0.201085070412']\n",
      "['SENSOR', '4', '1.03088856231', '-1.02236401255']\n",
      "['SENSOR', '5', '3.61390947737', '-1.83253751296']\n",
      "['ODOMETRY', '-0.00070658966091', '0.0999211581655', '-0.201215641668']\n",
      "['SENSOR', '8', '4.85461136115', '-0.441193806736']\n",
      "['SENSOR', '4', '1.05741780526', '-0.941454125061']\n",
      "['SENSOR', '5', '3.87262435523', '-1.67878747955']\n",
      "['ODOMETRY', '-0.000453651906009', '0.100058053226', '-0.0495339656427']\n",
      "['SENSOR', '8', '4.91108468826', '-0.314178952957']\n",
      "['SENSOR', '4', '0.889803683381', '-0.862587924879']\n",
      "['SENSOR', '5', '4.001907275', '-1.75368141539']\n",
      "['ODOMETRY', '-0.000374433876798', '0.0999632649737', '0.000227424570168']\n",
      "['SENSOR', '8', '4.89607963687', '-0.383905377422']\n",
      "['SENSOR', '4', '1.03529311279', '-1.08642084347']\n",
      "['SENSOR', '5', '3.6950450598', '-1.62958875652']\n",
      "['ODOMETRY', '0.000560387911335', '0.100022258169', '-0.000690144025918']\n",
      "['SENSOR', '8', '4.77811944922', '-0.344660362116']\n",
      "['SENSOR', '4', '0.6835522748', '-1.12873248686']\n",
      "['SENSOR', '5', '3.73078201728', '-1.73731680832']\n",
      "['ODOMETRY', '0.000302602191039', '0.0999189321218', '-0.000444970990895']\n",
      "['SENSOR', '8', '4.69613208187', '-0.387130423206']\n",
      "['SENSOR', '4', '0.787992521116', '-1.29156833847']\n",
      "['SENSOR', '5', '3.83067456717', '-1.73774870776']\n",
      "['ODOMETRY', '0.00033245853007', '0.099983722446', '3.29125408185e-05']\n",
      "['SENSOR', '8', '4.43987229987', '-0.336598652401']\n",
      "['SENSOR', '4', '0.824851032983', '-1.37173579521']\n",
      "['SENSOR', '5', '3.9011475333', '-1.83111676621']\n",
      "['ODOMETRY', '0.0003202499917', '0.0998020171825', '-0.000105496843961']\n",
      "['SENSOR', '8', '4.2295439355', '-0.31030833621']\n",
      "['SENSOR', '4', '0.562121580647', '-1.48855015673']\n",
      "['SENSOR', '5', '4.05047851622', '-1.85523162918']\n",
      "['ODOMETRY', '-0.00100295830048', '0.100070287738', '-0.000357513844835']\n",
      "['SENSOR', '8', '4.14141953883', '-0.401434359312']\n",
      "['SENSOR', '4', '0.698270524653', '-1.66266898697']\n",
      "['SENSOR', '5', '4.02646711957', '-1.85655804032']\n",
      "['ODOMETRY', '-0.000188063237756', '0.0999460648438', '-0.000367434508236']\n",
      "['SENSOR', '8', '4.49059154755', '-0.359642792629']\n",
      "['SENSOR', '4', '0.829352455695', '-1.82738715515']\n",
      "['SENSOR', '5', '3.9198043194', '-1.85847185083']\n",
      "['ODOMETRY', '0.000145108773369', '0.0999890308492', '-0.0008287350519']\n",
      "['SENSOR', '8', '4.0305518916', '-0.410490083997']\n",
      "['SENSOR', '4', '0.73104561841', '-1.91623346334']\n",
      "['SENSOR', '5', '3.95455389546', '-1.81930736734']\n",
      "['ODOMETRY', '0.000317286436698', '0.0999382739112', '-0.000139268224043']\n",
      "['SENSOR', '8', '3.94074309758', '-0.449033720052']\n",
      "['SENSOR', '4', '0.964327453377', '-2.01100457015']\n",
      "['SENSOR', '5', '4.12377150794', '-1.90702070002']\n",
      "['ODOMETRY', '-0.000489035127382', '0.10010816554', '-0.000185002568801']\n",
      "['SENSOR', '8', '3.91534330666', '-0.437399649948']\n",
      "['SENSOR', '4', '0.962844439912', '-2.09903655675']\n",
      "['SENSOR', '5', '4.0517244807', '-1.90944618344']\n",
      "['ODOMETRY', '-0.000448212051337', '0.100133168601', '-0.0012892610486']\n",
      "['SENSOR', '8', '3.8900635959', '-0.450094707651']\n",
      "['SENSOR', '4', '1.15499811144', '-2.14063500493']\n",
      "['SENSOR', '5', '4.15361685125', '-1.97034676565']\n",
      "['ODOMETRY', '-0.000333404857971', '0.0999912335863', '0.000192681272112']\n",
      "['SENSOR', '8', '3.77038644114', '-0.564054467112']\n",
      "['SENSOR', '4', '0.97476218836', '-2.24080497152']\n",
      "['SENSOR', '5', '4.17413980453', '-1.93610976395']\n",
      "['ODOMETRY', '0.000116797154257', '0.100137172993', '-0.000390404927584']\n",
      "['SENSOR', '8', '3.63226086454', '-0.540987639383']\n",
      "['SENSOR', '4', '1.10417906905', '-2.38354517123']\n",
      "['SENSOR', '5', '4.30329593999', '-2.0340943866']\n",
      "['SENSOR', '7', '4.94033277705', '-0.757833609985']\n",
      "['ODOMETRY', '-0.000590074857926', '0.0999133906589', '-0.000728548241666']\n",
      "['SENSOR', '8', '3.48279289223', '-0.442671033673']\n",
      "['SENSOR', '4', '1.21367576112', '-2.38923397243']\n",
      "['SENSOR', '5', '4.11351339144', '-2.12124027897']\n",
      "['SENSOR', '7', '4.9471083243', '-0.863368530901']\n",
      "['ODOMETRY', '0.000666027634388', '0.0999961749585', '-0.00041098921494']\n",
      "['SENSOR', '8', '3.31329949365', '-0.442208158787']\n",
      "['SENSOR', '4', '1.14087202904', '-2.42970915883']\n",
      "['SENSOR', '5', '4.28986062879', '-2.13951007973']\n",
      "['SENSOR', '7', '4.50725975692', '-0.916450378366']\n",
      "['ODOMETRY', '0.000123003750952', '0.0998768096926', '-0.000338091992771']\n",
      "['SENSOR', '8', '3.58031102591', '-0.631152189894']\n",
      "['SENSOR', '4', '1.28631748205', '-2.52280355497']\n",
      "['SENSOR', '5', '4.29694827231', '-2.13689954352']\n",
      "['SENSOR', '7', '4.75408087282', '-0.865684520747']\n",
      "['ODOMETRY', '-0.000346457382135', '0.100082898443', '0.000286890929699']\n",
      "['SENSOR', '8', '3.24455116705', '-0.576310948619']\n",
      "['SENSOR', '4', '1.27283382282', '-2.55880342549']\n",
      "['SENSOR', '5', '4.41483458904', '-2.05309918925']\n",
      "['SENSOR', '7', '4.63121738461', '-0.874521274014']\n",
      "['ODOMETRY', '-0.000955326333333', '0.100096022413', '-0.000648521461897']\n",
      "['SENSOR', '8', '3.24825762533', '-0.599421325277']\n",
      "['SENSOR', '4', '1.50961115908', '-2.52257610457']\n",
      "['SENSOR', '5', '4.51886453403', '-2.10172063614']\n",
      "['SENSOR', '7', '4.67172096933', '-0.894687907489']\n",
      "['ODOMETRY', '0.000425896177498', '0.100141662167', '0.000472712559654']\n",
      "['SENSOR', '8', '3.23972718873', '-0.58466657353']\n",
      "['SENSOR', '4', '1.74083901202', '-2.64636281352']\n",
      "['SENSOR', '5', '4.36236644466', '-2.15541169775']\n",
      "['SENSOR', '7', '4.54743703627', '-0.983253835746']\n",
      "['ODOMETRY', '0.000431170979157', '0.100025186646', '-9.68768691897e-05']\n",
      "['SENSOR', '8', '2.94713147949', '-0.538730214309']\n",
      "['SENSOR', '4', '1.59532499948', '-2.64189895638']\n",
      "['SENSOR', '5', '4.3940471103', '-2.19522353029']\n",
      "['SENSOR', '7', '4.58101568205', '-0.938466709816']\n",
      "['ODOMETRY', '-0.00126089300156', '0.100003408847', '-0.000253955323998']\n",
      "['SENSOR', '8', '3.01049631895', '-0.647455670209']\n",
      "['SENSOR', '4', '1.74223188952', '-2.64154107534']\n",
      "['SENSOR', '5', '4.6784242394', '-2.17124070517']\n",
      "['SENSOR', '7', '4.2652733295', '-0.996316702854']\n",
      "['ODOMETRY', '-0.000120654780893', '0.0999758007847', '-7.69033417334e-05']\n",
      "['SENSOR', '8', '2.81105029309', '-0.668553762428']\n",
      "['SENSOR', '4', '1.84304791933', '-2.65607644439']\n",
      "['SENSOR', '5', '4.63624935942', '-2.13548023805']\n",
      "['SENSOR', '7', '4.52016763586', '-0.991018307807']\n",
      "['ODOMETRY', '0.000189591679111', '0.100086348204', '-0.000323351005088']\n",
      "['SENSOR', '8', '2.93905607509', '-0.585176239548']\n",
      "['SENSOR', '4', '2.0438034489', '-2.72800462462']\n",
      "['SENSOR', '5', '4.52648542497', '-2.21078238898']\n",
      "['SENSOR', '7', '4.18292097158', '-1.01609850061']\n",
      "['ODOMETRY', '-0.000473674854813', '0.10002979275', '-7.07593512027e-05']\n",
      "['SENSOR', '8', '2.78937751626', '-0.681683822875']\n",
      "['SENSOR', '4', '1.99021011507', '-2.71901554183']\n",
      "['SENSOR', '5', '4.86030610026', '-2.19809066259']\n",
      "['SENSOR', '7', '4.08317192213', '-1.06263343315']\n",
      "['ODOMETRY', '-0.000361318872101', '0.0998516170993', '-0.000310786635761']\n",
      "['SENSOR', '8', '2.73109069077', '-0.664414233072']\n",
      "['SENSOR', '4', '2.02095934862', '-2.77281077247']\n",
      "['SENSOR', '5', '4.87996286947', '-2.20004470209']\n",
      "['SENSOR', '7', '4.22842676961', '-1.08754115393']\n",
      "['ODOMETRY', '-0.000269192638065', '0.100069154562', '0.000122327434553']\n",
      "['SENSOR', '8', '2.66996787215', '-0.741197858346']\n",
      "['SENSOR', '1', '4.92781804914', '0.165464879583']\n",
      "['SENSOR', '4', '2.28452084059', '-2.79103144813']\n",
      "['SENSOR', '5', '4.76222916544', '-2.3384152959']\n",
      "['SENSOR', '7', '4.19121425588', '-1.02506412666']\n",
      "['ODOMETRY', '0.000939233063028', '0.099978524037', '0.000685447903704']\n",
      "['SENSOR', '8', '2.51553389791', '-0.634533883027']\n",
      "['SENSOR', '1', '4.82706330783', '0.0213662625167']\n",
      "['SENSOR', '4', '2.37505836382', '-2.78320616069']\n",
      "['SENSOR', '5', '4.99415607057', '-2.22355221099']\n",
      "['SENSOR', '7', '3.97703512937', '-1.08364781079']\n",
      "['ODOMETRY', '-6.77419651127e-05', '0.100140978268', '-0.000135987996048']\n",
      "['SENSOR', '8', '2.23340888122', '-0.783411064361']\n",
      "['SENSOR', '1', '4.76673370222', '-0.00223170351281']\n",
      "['SENSOR', '4', '2.44121554947', '-2.76116600902']\n",
      "['SENSOR', '5', '4.814470924', '-2.31902709448']\n",
      "['SENSOR', '7', '4.04347080517', '-1.14617801134']\n",
      "['ODOMETRY', '0.00048886726123', '0.100041297815', '0.000679022990554']\n",
      "['SENSOR', '8', '2.30974435417', '-0.73023844975']\n",
      "['SENSOR', '1', '4.628923258', '0.0668774699138']\n",
      "['SENSOR', '4', '2.33202866839', '-2.80658070702']\n",
      "['SENSOR', '7', '4.1171835927', '-1.18735408115']\n",
      "['ODOMETRY', '-0.000763316825852', '0.0998626212511', '2.71380464145e-05']\n",
      "['SENSOR', '8', '2.30957175125', '-0.879774630027']\n",
      "['SENSOR', '1', '4.47241468075', '0.112481627522']\n",
      "['SENSOR', '4', '2.67200407606', '-2.88213261922']\n",
      "['SENSOR', '7', '4.13385546866', '-1.15907658911']\n",
      "['ODOMETRY', '0.00049292915547', '0.0998636041901', '-0.000379476978871']\n",
      "['SENSOR', '8', '2.36124708193', '-0.870145768712']\n",
      "['SENSOR', '1', '4.38735357247', '0.0955180040226']\n",
      "['SENSOR', '4', '2.65985478012', '-2.84310942138']\n",
      "['SENSOR', '7', '3.81181745959', '-1.20782384771']\n",
      "['ODOMETRY', '0.00033110801736', '0.100044850743', '-0.000577153724803']\n",
      "['SENSOR', '8', '2.41743004232', '-0.864072811384']\n",
      "['SENSOR', '1', '4.40194504238', '0.0658008097918']\n",
      "['SENSOR', '4', '2.94567913337', '-2.88562727445']\n",
      "['SENSOR', '7', '3.82544464355', '-1.2516790519']\n",
      "['ODOMETRY', '-0.000308672136194', '0.0998384881725', '2.64214654053e-05']\n",
      "['SENSOR', '8', '1.919908976', '-0.934813972655']\n",
      "['SENSOR', '1', '4.19345581376', '0.0757952107782']\n",
      "['SENSOR', '4', '2.94440761406', '-2.83450142815']\n",
      "['SENSOR', '7', '3.92107607518', '-1.15208824306']\n",
      "['ODOMETRY', '5.25016195751e-05', '0.0999941288634', '0.000406623957959']\n",
      "['SENSOR', '8', '1.9832859543', '-0.98442650848']\n",
      "['SENSOR', '1', '4.09215025882', '0.0717523426929']\n",
      "['SENSOR', '4', '3.05636372159', '-2.93752404177']\n",
      "['SENSOR', '7', '3.89565529492', '-1.28776086607']\n",
      "['ODOMETRY', '-0.000273197315602', '0.0998069485111', '-0.000285894067554']\n",
      "['SENSOR', '8', '2.12296121839', '-1.02645711564']\n",
      "['SENSOR', '1', '4.1786687521', '0.0497541354582']\n",
      "['SENSOR', '4', '3.22093748114', '-2.82397420009']\n",
      "['SENSOR', '7', '3.97449556221', '-1.29780080623']\n",
      "['ODOMETRY', '-0.000265608833175', '0.100026175106', '0.000137251905213']\n",
      "['SENSOR', '8', '2.05703216981', '-1.00685667222']\n",
      "['SENSOR', '1', '3.94149875624', '0.140569733461']\n",
      "['SENSOR', '4', '3.13725363665', '-2.87162377088']\n",
      "['SENSOR', '7', '3.76919105935', '-1.28829235854']\n",
      "['ODOMETRY', '0.000720910278101', '0.100023575271', '-0.000251711733041']\n",
      "['SENSOR', '8', '1.81579376561', '-1.08266616382']\n",
      "['SENSOR', '1', '3.67093999662', '0.0393555510994']\n",
      "['SENSOR', '4', '3.34596406803', '-2.97433102412']\n",
      "['SENSOR', '7', '3.90187420479', '-1.31346748714']\n",
      "['ODOMETRY', '0.000105339136838', '0.099882486807', '0.000711003741797']\n",
      "['SENSOR', '8', '1.81140030994', '-1.14228995915']\n",
      "['SENSOR', '1', '3.8261635831', '0.054006173831']\n",
      "['SENSOR', '4', '3.4796093187', '-2.98658709002']\n",
      "['SENSOR', '7', '3.82419437586', '-1.32957085243']\n",
      "['ODOMETRY', '-7.64579991343e-05', '0.0997922673878', '-0.000288040106145']\n",
      "['SENSOR', '8', '1.97927232141', '-1.27560024901']\n",
      "['SENSOR', '1', '3.72258806814', '0.161210114162']\n",
      "['SENSOR', '4', '3.3133245266', '-2.89891758969']\n",
      "['SENSOR', '7', '3.81183963458', '-1.32175712764']\n",
      "['ODOMETRY', '0.000385928532719', '0.0996806600309', '-0.200347125045']\n",
      "['SENSOR', '8', '1.71109074699', '-1.02714696631']\n",
      "['SENSOR', '1', '3.47474230224', '0.283685656426']\n",
      "['SENSOR', '4', '3.49665186449', '-2.71608754934']\n",
      "['SENSOR', '7', '3.86100523492', '-1.24660856692']\n",
      "['ODOMETRY', '1.18794066805e-06', '0.0997733761035', '-0.199977592699']\n",
      "['SENSOR', '8', '1.7913022851', '-0.988844108691']\n",
      "['SENSOR', '1', '3.3603196986', '0.438921773545']\n",
      "['SENSOR', '4', '3.59460143268', '-2.4995361102']\n",
      "['SENSOR', '7', '3.6769689176', '-1.0092059111']\n",
      "['ODOMETRY', '-0.000816556751862', '0.0997465372851', '-0.202314638633']\n",
      "['SENSOR', '8', '1.72700243897', '-0.727469713931']\n",
      "['SENSOR', '1', '3.41603446384', '0.795914407086']\n",
      "['SENSOR', '4', '3.74498201327', '-2.35790495186']\n",
      "['SENSOR', '7', '3.65858016772', '-0.852210047465']\n",
      "['ODOMETRY', '-0.00017107666944', '0.100202752156', '-0.199982427928']\n",
      "['SENSOR', '8', '1.59030504807', '-0.562457859173']\n",
      "['SENSOR', '1', '3.26977624121', '1.02025691453']\n",
      "['SENSOR', '4', '3.98177703193', '-2.11166828345']\n",
      "['SENSOR', '7', '3.60670130028', '-0.68064217983']\n",
      "['ODOMETRY', '0.000590926276013', '0.100545969921', '-0.199693808091']\n",
      "['SENSOR', '8', '1.42956896486', '-0.405210773852']\n",
      "['SENSOR', '1', '3.11530262765', '1.08617098938']\n",
      "['SENSOR', '4', '3.75792044617', '-1.94960186471']\n",
      "['SENSOR', '7', '3.44632621596', '-0.561375582074']\n",
      "['ODOMETRY', '-0.000298257136816', '0.0996631664365', '-0.198915852019']\n",
      "['SENSOR', '8', '1.41119753152', '-0.218291775829']\n",
      "['SENSOR', '1', '3.14171978452', '1.37417197214']\n",
      "['SENSOR', '4', '3.97964357546', '-1.84339806385']\n",
      "['SENSOR', '7', '3.37769084878', '-0.326486102096']\n",
      "['ODOMETRY', '-0.000223643982462', '0.0996312038275', '-0.2025564277']\n",
      "['SENSOR', '8', '1.25280389716', '-0.0478523885333']\n",
      "['SENSOR', '1', '3.28031792034', '1.55461890617']\n",
      "['SENSOR', '4', '3.96871903979', '-1.63320364602']\n",
      "['SENSOR', '7', '3.38773338161', '-0.165645510408']\n",
      "['ODOMETRY', '-0.000881424513507', '0.0999464043441', '-0.148617707433']\n",
      "['SENSOR', '8', '1.21794802013', '0.101499246355']\n",
      "['SENSOR', '1', '3.22237509743', '1.79072716463']\n",
      "['SENSOR', '4', '3.95617109814', '-1.44625205115']\n",
      "['SENSOR', '7', '3.44736574276', '0.0259459077986']\n",
      "['ODOMETRY', '6.73076006284e-05', '0.100132073471', '-0.000119445516902']\n",
      "['SENSOR', '8', '1.10085634846', '0.0128641004662']\n",
      "['SENSOR', '1', '3.15319314656', '1.83602899903']\n",
      "['SENSOR', '4', '3.96197077783', '-1.56149331217']\n",
      "['SENSOR', '7', '3.16144997508', '0.0497659131117']\n",
      "['ODOMETRY', '0.000216898596734', '0.100008870944', '8.44491124946e-05']\n",
      "['SENSOR', '8', '0.98718385021', '0.0505647532356']\n",
      "['SENSOR', '1', '3.44793567727', '1.89256285855']\n",
      "['SENSOR', '4', '3.92237270173', '-1.60631882591']\n",
      "['SENSOR', '7', '3.21973581934', '-0.0340907108888']\n",
      "['ODOMETRY', '0.000128009797301', '0.100043460223', '0.000987839174826']\n",
      "['SENSOR', '8', '0.89829332316', '0.135383956612']\n",
      "['SENSOR', '1', '3.27245387945', '1.88010743562']\n",
      "['SENSOR', '4', '3.96502932983', '-1.56994091616']\n",
      "['SENSOR', '7', '2.97710577284', '0.0715555315754']\n",
      "['ODOMETRY', '-0.000150318169053', '0.100071664514', '0.000144679939358']\n",
      "['SENSOR', '8', '0.774339649168', '-0.000516516086974']\n",
      "['SENSOR', '1', '3.48116649203', '1.98621024178']\n",
      "['SENSOR', '4', '3.85294043277', '-1.65568756419']\n",
      "['SENSOR', '7', '2.90643136834', '0.027023615806']\n",
      "['ODOMETRY', '-0.000465690711615', '0.100017674194', '-0.000142742436314']\n",
      "['SENSOR', '8', '0.527252645966', '0.172607270039']\n",
      "['SENSOR', '1', '3.3814040867', '1.97634147246']\n",
      "['SENSOR', '4', '4.10861734812', '-1.59369624478']\n",
      "['SENSOR', '7', '2.76353072157', '0.00840562337829']\n",
      "['ODOMETRY', '-0.000128208315622', '0.0999656883624', '0.000594809485977']\n",
      "['SENSOR', '8', '0.687305913164', '0.0798724661809']\n",
      "['SENSOR', '1', '3.21764284755', '1.9744178959']\n",
      "['SENSOR', '4', '4.08919040605', '-1.62141915547']\n",
      "['SENSOR', '7', '2.55473025478', '0.0977268515407']\n",
      "['ODOMETRY', '0.000821476656511', '0.0999158037258', '-0.00068566679204']\n",
      "['SENSOR', '8', '0.480616169488', '0.139441754953']\n",
      "['SENSOR', '1', '3.45668051094', '2.00189239553']\n",
      "['SENSOR', '4', '4.10524461182', '-1.67981251381']\n",
      "['SENSOR', '7', '2.33739775251', '0.0985219039275']\n",
      "['ODOMETRY', '0.000455722461866', '0.0999653812', '-4.23758429747e-05']\n",
      "['SENSOR', '8', '0.400741221794', '0.167751600491']\n",
      "['SENSOR', '1', '3.55108397687', '2.05110783316']\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "['SENSOR', '4', '4.06515935986', '-1.72057455144']\n",
      "['SENSOR', '7', '2.50287051778', '0.127838872546']\n",
      "['ODOMETRY', '0.000100633095148', '0.100219327349', '0.000631070639127']\n",
      "['SENSOR', '8', '0.417382565095', '0.188823271199']\n",
      "['SENSOR', '1', '3.35123641832', '2.09116664517']\n",
      "['SENSOR', '4', '3.95754572184', '-1.71268379099']\n",
      "['SENSOR', '7', '2.41072585607', '0.122504676512']\n",
      "['ODOMETRY', '-0.000607763710215', '0.100140453337', '-0.000218697666427']\n",
      "['SENSOR', '8', '0.369346735302', '0.271181092817']\n",
      "['SENSOR', '1', '3.67699620635', '2.07721447415']\n",
      "['SENSOR', '4', '4.07710876539', '-1.73456190265']\n",
      "['SENSOR', '7', '2.42576722134', '-0.0138112591739']\n",
      "['ODOMETRY', '-0.000139407170528', '0.0998511677938', '0.000334065170192']\n",
      "['SENSOR', '8', '0.335693906267', '0.462318775119']\n",
      "['SENSOR', '1', '3.54703844972', '2.08539235736']\n",
      "['SENSOR', '4', '3.94824350404', '-1.6777048794']\n",
      "['SENSOR', '7', '2.13297568426', '0.0826896741318']\n",
      "['ODOMETRY', '-0.000628418701319', '0.0999704949615', '-0.000124838470818']\n",
      "['SENSOR', '8', '0.0237407258792', '0.999084065632']\n",
      "['SENSOR', '1', '3.49005275028', '2.05858832887']\n",
      "['SENSOR', '4', '4.07307706178', '-1.72646904791']\n",
      "['SENSOR', '7', '2.07921259872', '0.00213504582166']\n",
      "['ODOMETRY', '-9.78009794874e-05', '0.100003974246', '-0.00134217319887']\n",
      "['SENSOR', '8', '0.129747875719', '2.20808064967']\n",
      "['SENSOR', '1', '3.82990788235', '2.17088111054']\n",
      "['SENSOR', '4', '4.01274770551', '-1.80022287123']\n",
      "['SENSOR', '7', '1.74885650734', '-0.0121223549881']\n",
      "['ODOMETRY', '0.000936885880489', '0.0999064874362', '0.000600172698976']\n",
      "['SENSOR', '8', '0.00599324163092', '2.67223936752']\n",
      "['SENSOR', '1', '3.82421795548', '2.19552984414']\n",
      "['SENSOR', '3', '4.79851180987', '0.60594351723']\n",
      "['SENSOR', '4', '4.05133471121', '-1.81575940442']\n",
      "['SENSOR', '7', '1.85931144483', '0.164227994673']\n",
      "['ODOMETRY', '-0.000253202498002', '0.10008722051', '0.000209063753791']\n",
      "['SENSOR', '8', '0.355098876696', '2.89500927945']\n",
      "['SENSOR', '1', '3.90659962847', '2.1333112384']\n",
      "['SENSOR', '3', '4.81068150996', '0.62679112096']\n",
      "['SENSOR', '4', '4.09111006731', '-1.88931307412']\n",
      "['SENSOR', '7', '1.79380510621', '-0.0357020447254']\n",
      "['ODOMETRY', '0.000157129099918', '0.100032988722', '-0.000729862285518']\n",
      "['SENSOR', '8', '0.310596091371', '2.80972354331']\n",
      "['SENSOR', '1', '3.70515903504', '2.23033190653']\n",
      "['SENSOR', '3', '4.65126987644', '0.73166186817']\n",
      "['SENSOR', '4', '4.23935852045', '-1.93054147639']\n",
      "['SENSOR', '7', '1.76174667646', '0.0262704139452']\n",
      "['ODOMETRY', '-0.000548408308394', '0.0999919238562', '-0.000250033911117']\n",
      "['SENSOR', '8', '0.672273832166', '3.01287570212']\n",
      "['SENSOR', '1', '3.82284974892', '2.26851849591']\n",
      "['SENSOR', '3', '4.66661981658', '0.750783246804']\n",
      "['SENSOR', '4', '4.20611841782', '-1.87591401282']\n",
      "['SENSOR', '7', '1.65083446213', '-0.10214389997']\n",
      "['ODOMETRY', '-0.000371751330884', '0.0999313796848', '0.000860224474773']\n",
      "['SENSOR', '8', '0.556611108037', '3.00403808456']\n",
      "['SENSOR', '1', '4.07609954779', '2.29923731693']\n",
      "['SENSOR', '3', '4.61268174855', '0.746177069223']\n",
      "['SENSOR', '4', '4.26511409215', '-1.93342631232']\n",
      "['SENSOR', '7', '1.5749019735', '0.109768246387']\n",
      "['ODOMETRY', '2.93556084258e-05', '0.100060189388', '0.00098238378032']\n",
      "['SENSOR', '8', '0.731212089539', '3.13442910945']\n",
      "['SENSOR', '1', '4.19104478917', '2.19441500315']\n",
      "['SENSOR', '3', '4.50331304373', '0.707514185573']\n",
      "['SENSOR', '4', '4.34762941195', '-1.97920192321']\n",
      "['SENSOR', '7', '1.33294282082', '0.0230575892315']\n",
      "['ODOMETRY', '1.97994777393e-05', '0.099913771488', '0.000324267521873']\n",
      "['SENSOR', '8', '0.726506378949', '3.06634580351']\n",
      "['SENSOR', '1', '4.04114278918', '2.33975587408']\n",
      "['SENSOR', '3', '4.50535713636', '0.78187843772']\n",
      "['SENSOR', '4', '4.29965089498', '-1.9407687623']\n",
      "['SENSOR', '7', '1.27944364032', '0.0552717851415']\n",
      "['ODOMETRY', '-0.000438875804674', '0.099903540815', '0.000489960444763']\n",
      "['SENSOR', '8', '0.62179737963', '3.10384573817']\n",
      "['SENSOR', '1', '4.24151843231', '2.38316523462']\n",
      "['SENSOR', '3', '4.26416968603', '0.789449674711']\n",
      "['SENSOR', '4', '4.33240098534', '-1.96547159788']\n",
      "['SENSOR', '7', '1.24198454896', '-0.0312764055356']\n",
      "['ODOMETRY', '0.00124083409796', '0.09994562889', '-0.000946313040158']\n",
      "['SENSOR', '8', '0.760293224516', '3.02148565162']\n",
      "['SENSOR', '1', '4.28994508486', '2.2237346564']\n",
      "['SENSOR', '3', '4.26717238324', '0.896293534553']\n",
      "['SENSOR', '4', '4.27865244716', '-2.06179292551']\n",
      "['SENSOR', '7', '1.12266271613', '0.0415462318364']\n",
      "['ODOMETRY', '0.000129299270017', '0.100041869119', '-0.000655703667229']\n",
      "['SENSOR', '1', '4.45232713374', '2.39852116851']\n",
      "['SENSOR', '3', '4.28823399159', '0.800783040336']\n",
      "['SENSOR', '4', '4.51068601353', '-2.05307475783']\n",
      "['SENSOR', '7', '0.901779605434', '0.0337725706528']\n",
      "['SENSOR', '8', '0.977088757516', '3.04355799617']\n",
      "['SENSOR', '9', '5.04440283106', '-0.0103650380772']\n",
      "['ODOMETRY', '0.000537988723435', '0.0998524709068', '-0.000469990594849']\n",
      "['SENSOR', '1', '4.33243080487', '2.33948279246']\n",
      "['SENSOR', '3', '4.10533364383', '0.863859372451']\n",
      "['SENSOR', '4', '4.60752865229', '-2.16099524972']\n",
      "['SENSOR', '5', '5.14915760553', '-1.41243997907']\n",
      "['SENSOR', '7', '1.00236861922', '0.0941916748689']\n",
      "['SENSOR', '8', '1.23019120912', '3.1121174408']\n",
      "['SENSOR', '9', '4.82508017193', '0.051751886788']\n",
      "['ODOMETRY', '-0.000205828817179', '0.100018336841', '0.000249542798678']\n",
      "['SENSOR', '1', '4.33283896214', '2.46058071055']\n",
      "['SENSOR', '3', '4.18312790586', '0.872387916195']\n",
      "['SENSOR', '4', '4.50465419667', '-2.16382123772']\n",
      "['SENSOR', '5', '5.10336341986', '-1.39939623537']\n",
      "['SENSOR', '7', '0.704062055735', '0.108392416192']\n",
      "['SENSOR', '8', '1.20229516344', '3.16630582891']\n",
      "['SENSOR', '9', '4.60881937906', '0.0659418142304']\n",
      "['ODOMETRY', '-4.84769033309e-05', '0.0998816841886', '-0.000516660455681']\n",
      "['SENSOR', '1', '4.50934992142', '2.36066884164']\n",
      "['SENSOR', '3', '4.00995160209', '0.872702584178']\n",
      "['SENSOR', '4', '4.6556628157', '-2.10991460657']\n",
      "['SENSOR', '5', '5.19236777304', '-1.46894121337']\n",
      "['SENSOR', '7', '0.595140527124', '-0.00247210771207']\n",
      "['SENSOR', '8', '1.1409026588', '3.07525098835']\n",
      "['SENSOR', '9', '4.84639214222', '-0.0548253969277']\n",
      "['ODOMETRY', '5.67295197279e-05', '0.100061981172', '0.000311855228904']\n",
      "['SENSOR', '1', '4.62274843795', '2.4285094599']\n",
      "['SENSOR', '3', '3.816414436', '0.903117907018']\n",
      "['SENSOR', '4', '4.36708385287', '-2.09249135014']\n",
      "['SENSOR', '5', '5.02112185456', '-1.48480085145']\n",
      "['SENSOR', '7', '0.722330360765', '0.131951761725']\n",
      "['SENSOR', '8', '1.42401805985', '3.15417376672']\n",
      "['SENSOR', '9', '4.47099457601', '-0.0163240457546']\n",
      "['ODOMETRY', '0.00115916483704', '0.0999765525792', '-5.49301680053e-05']\n",
      "['SENSOR', '1', '4.63927554993', '2.3761844012']\n",
      "['SENSOR', '3', '3.9376280419', '0.858518143986']\n",
      "['SENSOR', '4', '4.78153044206', '-2.1711021116']\n",
      "['SENSOR', '5', '5.01779109603', '-1.4755448089']\n",
      "['SENSOR', '7', '0.400487152841', '0.0954002442539']\n",
      "['SENSOR', '8', '1.5705356409', '2.92820511279']\n",
      "['SENSOR', '9', '4.55151136998', '0.00907004840239']\n",
      "['ODOMETRY', '-0.00015974321687', '0.100001954963', '0.000375007480502']\n",
      "['SENSOR', '1', '4.70778012293', '2.42157051538']\n",
      "['SENSOR', '3', '3.97505453196', '0.932447245049']\n",
      "['SENSOR', '4', '4.70850325866', '-2.17152767966']\n",
      "['SENSOR', '5', '4.95124738508', '-1.40632188062']\n",
      "['SENSOR', '7', '0.383618192004', '0.149257977921']\n",
      "['SENSOR', '8', '1.42134250744', '3.07398092328']\n",
      "['SENSOR', '9', '4.4606042563', '-0.0161739269736']\n",
      "['ODOMETRY', '-0.000584883749866', '0.0998918193766', '0.000164194828446']\n",
      "['SENSOR', '1', '4.79178293375', '2.48566706534']\n",
      "['SENSOR', '3', '3.63177076576', '0.880780831994']\n",
      "['SENSOR', '4', '5.01568989921', '-2.26708689866']\n",
      "['SENSOR', '5', '4.98135878948', '-1.51500189459']\n",
      "['SENSOR', '7', '0.270711489516', '0.323697074147']\n",
      "['SENSOR', '8', '1.83521291936', '3.07684872323']\n",
      "['SENSOR', '9', '4.49376890802', '0.0459102111966']\n",
      "['ODOMETRY', '0.000303680491478', '0.100162153889', '9.76063766532e-05']\n",
      "['SENSOR', '1', '4.87239258496', '2.49373595981']\n",
      "['SENSOR', '3', '3.63961037672', '0.972600323588']\n",
      "['SENSOR', '4', '5.06493994808', '-2.19537045826']\n",
      "['SENSOR', '5', '4.87043159977', '-1.5187934091']\n",
      "['SENSOR', '7', '0.158485679685', '0.430892408212']\n",
      "['SENSOR', '8', '1.94404523721', '3.02999537927']\n",
      "['SENSOR', '9', '4.25502389275', '0.0807923140829']\n",
      "['ODOMETRY', '-0.000118594429945', '0.100038226107', '0.0016860488551']\n",
      "['SENSOR', '3', '3.85525315557', '0.972472209298']\n",
      "['SENSOR', '4', '4.7395963428', '-2.22231607732']\n",
      "['SENSOR', '5', '5.03393746044', '-1.59221850796']\n",
      "['SENSOR', '6', '5.09689220959', '-0.905474203378']\n",
      "['SENSOR', '7', '0.210352002822', '1.03187235077']\n",
      "['SENSOR', '8', '2.18673487592', '3.14059664668']\n",
      "['SENSOR', '9', '4.10712293299', '0.0906600190361']\n"
     ]
    }
   ],
   "source": [
    "f = open('/data files/sensor_data_ekf.dat')\n",
    "for line in f:\n",
    "    q = line.split()\n",
    "    print(q)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d835e1d5",
   "metadata": {},
   "source": [
    "Считывание координат датчиков"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "37d2eaf2",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[1, 2, 1], [2, 0, 4], [3, 2, 7], [4, 9, 2], [5, 10, 5], [6, 9, 8], [7, 5, 5], [8, 5, 3], [9, 5, 9]]\n"
     ]
    }
   ],
   "source": [
    "L = []\n",
    "f = open('/data files/landmarks.dat')\n",
    "for line in f:\n",
    "    L.append( [int(x) for x in line.split()])\n",
    "print(L)\n",
    "number_of_sensors = len(L)*2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "7ee36b23",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[4 6 6]\n"
     ]
    }
   ],
   "source": [
    "A = np.array([[1,2,3],[4,5,6],[7,8,9]])\n",
    "print(A[1:3][0] + np.array([0,1,0]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "e3f232b0",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[1, 2, 1]\n"
     ]
    }
   ],
   "source": [
    "print(L[0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "34f26142",
   "metadata": {},
   "outputs": [],
   "source": [
    "def conc(Y,x):\n",
    "    if len(Y) == 0:\n",
    "        return x\n",
    "    else:\n",
    "        return np.vstack((Y,x))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "4da5ec07",
   "metadata": {},
   "outputs": [],
   "source": [
    "def cholesky(P):\n",
    "    n = len(P)\n",
    "    A = [[0.0] * n for i in range(0,n)]\n",
    "    for i in range(0,n):\n",
    "        sum =0 \n",
    "        for k in range(0,i):\n",
    "            sum+= A[i][k]*A[i][k]\n",
    "        A[i][i] = math.sqrt(P[i][i] - sum)\n",
    "        for j in range(i+1,n):\n",
    "            sum =0\n",
    "            for k in range(0,i):\n",
    "                sum += A[j][k] * A[i][k]\n",
    "            A[j][i] = (P[j][i] - sum)/A[i][i]\n",
    "    return A"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "id": "303b5cb3",
   "metadata": {},
   "outputs": [],
   "source": [
    "def nrml(ang):\n",
    "    while ang >  math.pi:\n",
    "        ang-= 2*math.pi\n",
    "    while ang < -math.pi:\n",
    "        ang+= 2*math.pi\n",
    "    return ang"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "id": "3ec43b82",
   "metadata": {},
   "outputs": [],
   "source": [
    "def gensigma(n,m,P):\n",
    "    sqP = cholesky(P)\n",
    "    sigma = np.array([[0.0] * n for i in range(0,2 * n + 1)])\n",
    "    sigma[0] = m\n",
    "    for i in range(1,n+1 ):\n",
    "        sigma[i] = m + math.sqrt(n + delta )* np.array(sqP[:][i - 1])\n",
    "        sigma[i+n] = m - math.sqrt( n + delta ) * np.array(sqP[:][i - 1])\n",
    "    return sigma\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b4f91222",
   "metadata": {},
   "source": [
    "# Сигма точечный фильтр для расстояний"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "id": "668ae30e",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "num = \n",
      "1\n",
      "ODOMETRY 0.101373685667 0.0996252285934 0.000862235559071\n",
      "\n",
      "num = \n",
      "2\n",
      "SENSOR 1 2.29725290252 0.353956815014\n",
      "\n",
      "num = \n",
      "3\n",
      "SENSOR 2 4.01825728983 1.51507415344\n",
      "\n",
      "num = \n",
      "4\n",
      "ODOMETRY 0.101823187296 0.0997248889496 -0.000206654546125\n",
      "\n",
      "sqP = \n",
      "[[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]\n",
      "sigma = \n",
      "[[ 0.  0.  0.]\n",
      " [ 2.  0.  0.]\n",
      " [ 0.  2.  0.]\n",
      " [ 0.  0.  2.]\n",
      " [-2.  0.  0.]\n",
      " [ 0. -2.  0.]\n",
      " [ 0.  0. -2.]]\n",
      "sigmax = \n",
      "[[ 0.10223592  0.09911376  0.01008209]\n",
      " [ 2.10223592 -0.05041349  0.08592826]\n",
      " [ 0.10223592  2.09911376  0.01008209]\n",
      " [ 0.10223592  0.09911376  2.01008209]\n",
      " [-1.89776408 -0.03207826 -0.09431952]\n",
      " [ 0.10223592 -1.90088624  0.01008209]\n",
      " [ 0.10223592  0.09911376 -1.98991791]]\n",
      "sqP = \n",
      "[[1.0954451150103324, 0.0, 0.0], [-0.0041844252011476535, 1.0971314448189138, 0.0], [0.04113573874831966, 0.00031110853161275923, 1.0956169748066202]]\n",
      "m = \n",
      "[0.10567613 0.21387904 0.11335925]\n",
      "P = \n",
      "[[ 1.19860396 -0.00321138  0.00802   ]\n",
      " [-0.00321138  0.4049782  -0.04441321]\n",
      " [ 0.00802    -0.04441321  0.2109865 ]]\n",
      "num = \n",
      "5\n",
      "SENSOR 1 1.96846687533 0.276209105501\n",
      "\n",
      "num = \n",
      "6\n",
      "SENSOR 2 3.87986704568 1.40393072429\n",
      "\n",
      "num = \n",
      "7\n",
      "ODOMETRY 0.100901695741 0.0997582630735 -0.000381616354179\n",
      "\n",
      "sqP = \n",
      "[[1.0948077267744192, 0.0, 0.0], [-0.0029332806808743403, 0.6363722176942047, 0.0], [0.007325485849216286, -0.06975747510309449, 0.45394573807980626]]\n",
      "sigma = \n",
      "[[ 0.10567613  0.21387904  0.11335925]\n",
      " [ 2.29529158  0.21387904  0.11335925]\n",
      " [ 0.09980957  1.48662347  0.11335925]\n",
      " [ 0.1203271   0.07436409  1.02125072]\n",
      " [-2.08393933  0.21387904  0.11335925]\n",
      " [ 0.11154269 -1.0588654   0.11335925]\n",
      " [ 0.09102516  0.35339399 -0.79453223]]\n",
      "sigmax = \n",
      "[[ 0.20729266  0.31146474  0.13390392]\n",
      " [ 2.39690811  0.14053719  0.18093163]\n",
      " [ 0.2014261   1.58432802  0.13333108]\n",
      " [ 0.22194363  0.17163833  1.04322286]\n",
      " [-1.98232279  0.17400709  0.02195202]\n",
      " [ 0.21315922 -0.9614019   0.13447605]\n",
      " [ 0.19264169  0.45127021 -0.77541944]]\n",
      "sqP = \n",
      "[[1.1826521997119233, 0.0, 0.0], [-0.00975749445824989, 0.7838149514675827, 0.0], [0.039609679948303056, -0.03951721017357959, 0.6366812514992659]]\n",
      "m = \n",
      "[0.21025875 0.33560079 0.18676532]\n",
      "P = \n",
      "[[ 1.39759965 -0.00717346  0.03036946]\n",
      " [-0.00717346  0.22493095 -0.03922013]\n",
      " [ 0.03036946 -0.03922013  0.1387534 ]]\n",
      "num = \n",
      "8\n",
      "SENSOR 1 1.96852886749 0.242784804425\n",
      "\n",
      "num = \n",
      "9\n",
      "SENSOR 2 3.87971065887 1.32647676387\n",
      "\n",
      "num = \n",
      "10\n",
      "ODOMETRY 0.0997651394437 0.0997993534855 -0.000208097310853\n",
      "\n",
      "sqP = \n",
      "[[1.1822011863532467, 0.0, 0.0], [-0.006067880396898903, 0.47423003873150954, 0.0], [0.025688911068478197, -0.08237405258407988, 0.3623644491587537]]\n",
      "sigma = \n",
      "[[ 0.21025875  0.33560079  0.18676532]\n",
      " [ 2.57466112  0.33560079  0.18676532]\n",
      " [ 0.19812299  1.28406087  0.18676532]\n",
      " [ 0.26163657  0.17085269  0.91149422]\n",
      " [-2.15414362  0.33560079  0.18676532]\n",
      " [ 0.22239451 -0.61285929  0.18676532]\n",
      " [ 0.15888093  0.5003489  -0.53796358]]\n",
      "sigmax = \n",
      "[[ 0.31077883  0.43056856  0.21730766]\n",
      " [ 2.6751812   0.24648082  0.231591  ]\n",
      " [ 0.29864307  1.37939229  0.21615294]\n",
      " [ 0.36215665  0.26412663  0.94687335]\n",
      " [-2.05362354  0.28931824  0.09839317]\n",
      " [ 0.32291459 -0.51826916  0.21845789]\n",
      " [ 0.25940101  0.59675985 -0.51233863]]\n",
      "sqP = \n",
      "[[1.264237471497906, 0.0, 0.0], [-0.013981198177476816, 0.6609563112086069, 0.0], [0.038554106967814736, -0.0436225128205671, 0.5755987859485313]]\n",
      "m = \n",
      "[0.31079793 0.35197671 0.19225049]\n",
      "P = \n",
      "[[ 1.5972913  -0.0123658   0.03508605]\n",
      " [-0.0123658   0.18474379 -0.02979166]\n",
      " [ 0.03508605 -0.02979166  0.12569676]]\n",
      "num = \n",
      "11\n",
      "SENSOR 1 1.87508331126 0.0936569272975\n",
      "\n",
      "num = \n",
      "12\n",
      "SENSOR 2 3.849192179 1.20772931615\n",
      "\n",
      "num = \n",
      "13\n",
      "ODOMETRY 0.0988021399415 0.100360366225 -0.00122883669589\n",
      "\n",
      "sqP = \n",
      "[[1.2638399034491004, 0.0, 0.0], [-0.009784311294875139, 0.4297069445152456, 0.0], [0.027761468626871205, -0.06869805711492072, 0.346708289868223]]\n",
      "sigma = \n",
      "[[ 0.31079793  0.35197671  0.19225049]\n",
      " [ 2.83847774  0.35197671  0.19225049]\n",
      " [ 0.29122931  1.2113906   0.19225049]\n",
      " [ 0.36632087  0.2145806   0.88566707]\n",
      " [-2.21688188  0.35197671  0.19225049]\n",
      " [ 0.33036655 -0.50743718  0.19225049]\n",
      " [ 0.25527499  0.48937283 -0.50116609]]\n",
      "sigmax = \n",
      "[[ 0.41035497  0.44348236  0.23208298]\n",
      " [ 2.93803478  0.25423367  0.2124051 ]\n",
      " [ 0.39078635  1.30365815  0.23028483]\n",
      " [ 0.46587791  0.30373476  0.93051623]\n",
      " [-2.11732483  0.30012628  0.10697773]\n",
      " [ 0.42992359 -0.41672846  0.23386588]\n",
      " [ 0.35483204  0.58294795 -0.46647304]]\n",
      "sqP = \n",
      "[[1.3409540385255816, 0.0, 0.0], [-0.015396703141467565, 0.6284365560277847, 0.0], [0.03207806041835974, -0.035510831325141164, 0.5668872272568126]]\n",
      "m = \n",
      "[0.41054639 0.40821237 0.22026876]\n",
      "P = \n",
      "[[ 1.79742476 -0.01543463  0.03202037]\n",
      " [-0.01543463  0.17469919 -0.02625941]\n",
      " [ 0.03202037 -0.02625941  0.1223973 ]]\n",
      "num = \n",
      "14\n",
      "SENSOR 1 1.64421872619 -0.0593514730712\n",
      "\n",
      "num = \n",
      "15\n",
      "SENSOR 2 3.95417280741 1.25970767463\n",
      "\n",
      "num = \n",
      "16\n",
      "ODOMETRY 0.0998731020684 0.0998098334361 0.000926085589796\n",
      "\n",
      "sqP = \n",
      "[[1.3406807094582511, 0.0, 0.0], [-0.01151253139545014, 0.41781174123802867, 0.0], [0.02388366222404374, -0.06219175587682158, 0.3434516759841863]]\n",
      "sigma = \n",
      "[[ 0.41054639  0.40821237  0.22026876]\n",
      " [ 3.09190781  0.40821237  0.22026876]\n",
      " [ 0.38752133  1.24383585  0.22026876]\n",
      " [ 0.45831371  0.28382886  0.90717211]\n",
      " [-2.27081503  0.40821237  0.22026876]\n",
      " [ 0.43357145 -0.42741111  0.22026876]\n",
      " [ 0.36277907  0.53259588 -0.46663459]]\n",
      "sigmax = \n",
      "[[ 0.50811969  0.49583323  0.26920534]\n",
      " [-3.0937042   0.30797304  0.21534131]\n",
      " [ 0.48509463  1.33256015  0.26717507]\n",
      " [ 0.55588702  0.36901309  0.96023669]\n",
      " [-2.17324173  0.35144392  0.13750678]\n",
      " [ 0.53114476 -0.34094015  0.27120966]\n",
      " [ 0.46035237  0.62245347 -0.42193766]]\n",
      "sqP = \n",
      "[[1.4505884979024355, 0.0, 0.0], [0.06480292326157043, 0.6165572612861334, 0.0], [0.04027738234041348, -0.03608532820454601, 0.5643304703280464]]\n",
      "m = \n",
      "[-0.26635165  0.59536379  0.19601846]\n",
      "P = \n",
      "[[ 2.10066433  0.07192095  0.04278035]\n",
      " [ 0.07192095  0.17503267 -0.02068919]\n",
      " [ 0.04278035 -0.02068919  0.12246141]]\n",
      "num = \n",
      "17\n",
      "SENSOR 1 1.64079501221 -0.137553375677\n",
      "\n",
      "num = \n",
      "18\n",
      "SENSOR 2 3.59759872767 1.04090139691\n",
      "\n",
      "num = \n",
      "19\n",
      "ODOMETRY 0.100314651053 0.0996200579912 0.000575268519869\n",
      "\n",
      "sqP = \n",
      "[[1.4493668719986688, 0.0, 0.0], [0.049622325394166125, 0.415415805307997, 0.0], [0.029516576017586376, -0.05332939822207598, 0.3445956435555329]]\n",
      "sigma = \n",
      "[[-0.26635165  0.59536379  0.19601846]\n",
      " [ 2.6323821   0.59536379  0.19601846]\n",
      " [-0.167107    1.4261954   0.19601846]\n",
      " [-0.2073185   0.488705    0.88520974]\n",
      " [-3.16508539  0.59536379  0.19601846]\n",
      " [-0.3655963  -0.23546782  0.19601846]\n",
      " [-0.3253848   0.70202259 -0.49317283]]\n",
      "sigmax = \n",
      "[[-0.16555246  0.6937937   0.17947891]\n",
      " [ 2.73318128  0.50379978  0.23574293]\n",
      " [-0.06630781  1.52577973  0.18931291]\n",
      " [-0.10651931  0.58793926  0.87450626]\n",
      " [-3.0642862   0.49584496  0.18840236]\n",
      " [-0.26479711 -0.13916103  0.16980768]\n",
      " [-0.22458561  0.79930522 -0.51549082]]\n",
      "sqP = \n",
      "[[1.517892596030606, 0.0, 0.0], [0.014478771288699245, 0.6187706188995976, 0.0], [0.018217644445826707, -0.02869508930268372, 0.5656527592928674]]\n",
      "m = \n",
      "[-0.15999016  0.60922538  0.37938716]\n",
      "P = \n",
      "[[ 2.30366108  0.01729264  0.02084755]\n",
      " [ 0.01729264  0.18578025 -0.01952096]\n",
      " [ 0.02084755 -0.01952096  0.1187613 ]]\n",
      "num = \n",
      "20\n",
      "SENSOR 1 1.63668963967 -0.164205155951\n",
      "\n",
      "num = \n",
      "21\n",
      "SENSOR 2 3.76733107867 1.02783331425\n",
      "\n",
      "num = \n",
      "22\n",
      "ODOMETRY 0.100066427373 0.0997787196014 0.00016140029208\n",
      "\n",
      "sqP = \n",
      "[[1.5177816309497534, 0.0, 0.0], [0.011393364016705817, 0.43087172281598485, 0.0], [0.013735541833491485, -0.045668935151996264, 0.34130189564741603]]\n",
      "sigma = \n",
      "[[-0.15999016  0.60922538  0.37938716]\n",
      " [ 2.8755731   0.60922538  0.37938716]\n",
      " [-0.13720344  1.47096882  0.37938716]\n",
      " [-0.13251908  0.51788751  1.06199095]\n",
      " [-3.19555343  0.60922538  0.37938716]\n",
      " [-0.18277689 -0.25251807  0.37938716]\n",
      " [-0.18746125  0.70056325 -0.30321663]]\n",
      "sigmax = \n",
      "[[-0.05910024  0.70866811  0.37344581]\n",
      " [ 2.97646302  0.51096988  0.39581925]\n",
      " [-0.03631352  1.57052111  0.37571313]\n",
      " [-0.03162916  0.61745591  1.0587833 ]\n",
      " [-3.09466351  0.50971233  0.37477104]\n",
      " [-0.08188697 -0.15323653  0.37118157]\n",
      " [-0.08657133  0.79980526 -0.3118872 ]]\n",
      "sqP = \n",
      "[[1.5823967748644998, 0.0, 0.0], [0.003008636482193965, 0.6286107750627518, 0.0], [0.008029769333835096, -0.024810490570732736, 0.5628466540199116]]\n",
      "m = \n",
      "[-0.05948529  0.64796393  0.35348455]\n",
      "P = \n",
      "[[ 2.50393383  0.00375093  0.00988171]\n",
      " [ 0.00375093  0.17673729 -0.01312113]\n",
      " [ 0.00988171 -0.01312113  0.12156332]]\n",
      "num = \n",
      "23\n",
      "SENSOR 1 1.35096574442 -0.290175121698\n",
      "\n",
      "num = \n",
      "24\n",
      "SENSOR 2 3.77521988295 0.983315510779\n",
      "\n",
      "num = \n",
      "25\n",
      "ODOMETRY 0.100757002056 0.0998428685737 -0.000211648847469\n",
      "\n",
      "sqP = \n",
      "[[1.5823823264984664, 0.0, 0.0], [0.00237043103179584, 0.4203946571132847, 0.0], [0.006244832908870283, -0.03124665824498785, 0.3472001893075724]]\n",
      "sigma = \n",
      "[[-0.05948529  0.64796393  0.35348455]\n",
      " [ 3.10527936  0.64796393  0.35348455]\n",
      " [-0.05474443  1.48875324  0.35348455]\n",
      " [-0.04699562  0.58547061  1.04788493]\n",
      " [-3.22424994  0.64796393  0.35348455]\n",
      " [-0.06422615 -0.19282539  0.35348455]\n",
      " [-0.07197495  0.71045725 -0.34091583]]\n",
      "sigmax = \n",
      "[[ 0.04074254  0.7476605   0.35753257]\n",
      " [-3.07767812  0.54838791  0.34712765]\n",
      " [ 0.0454834   1.5884295   0.35800517]\n",
      " [ 0.0532322   0.68510885  1.05317778]\n",
      " [-3.12402211  0.54820033  0.35174758]\n",
      " [ 0.03600168 -0.09311074  0.35705988]\n",
      " [ 0.02825287  0.8101966  -0.33811326]]\n",
      "sqP = \n",
      "[[1.4320368980842366, 0.0, 0.0], [0.08256525607670956, 0.6150909255795017, 0.0], [0.004837533891359354, -0.017679696567878753, 0.566267053677732]]\n",
      "m = \n",
      "[-0.72433777  0.84664172  0.38226283]\n",
      "P = \n",
      "[[ 2.0470523   0.09108855  0.00431738]\n",
      " [ 0.09108855  0.18295404 -0.01095685]\n",
      " [ 0.00431738 -0.01095685  0.12109389]]\n",
      "num = \n",
      "26\n",
      "SENSOR 8 4.9185663896 -0.395438658587\n",
      "\n",
      "num = \n",
      "27\n",
      "SENSOR 1 1.34647237204 -0.509755484258\n",
      "\n",
      "num = \n",
      "28\n",
      "SENSOR 2 3.71427679672 0.92294567505\n",
      "\n",
      "num = \n",
      "29\n",
      "ODOMETRY 0.100962422572 0.0998534959103 0.000658662316867\n",
      "\n",
      "sqP = \n",
      "[[1.4307523550649428, 0.0, 0.0], [0.06366479193904094, 0.4229667074845116, 0.0], [0.003017562681241362, -0.026358955582007473, 0.3469726085899958]]\n",
      "sigma = \n",
      "[[-7.24337771e-01  8.46641723e-01  3.82262832e-01]\n",
      " [ 2.13716694e+00  8.46641723e-01  3.82262832e-01]\n",
      " [-5.97008187e-01  1.69257514e+00  3.82262832e-01]\n",
      " [-7.18302646e-01  7.93923811e-01  1.07620805e+00]\n",
      " [-3.58584248e+00  8.46641723e-01  3.82262832e-01]\n",
      " [-8.51667355e-01  7.08307597e-04  3.82262832e-01]\n",
      " [-7.30372896e-01  8.99359634e-01 -3.11682386e-01]]\n",
      "sigmax = \n",
      "[[-0.62379242  0.92769343  0.32396001]\n",
      " [ 2.23771229  0.78486575  0.4606996 ]\n",
      " [-0.49646283  1.78037433  0.33472442]\n",
      " [-0.61775729  0.87532591  1.01839545]\n",
      " [ 2.79788818  0.75263126  0.41588771]\n",
      " [-0.751122    0.07370024  0.31413959]\n",
      " [-0.62982754  0.98005801 -0.3704733 ]]\n",
      "sqP = \n",
      "[[1.4402171539873185, 0.0, 0.0], [-0.04689638190666367, 0.6207424335192105, 0.0], [0.04656357762551348, -0.01291933191301438, 0.5664106313243383]]\n",
      "m = \n",
      "[0.16540402 0.89992155 0.41123892]\n",
      "P = \n",
      "[[ 2.07166422 -0.04979388  0.0518979 ]\n",
      " [-0.04979388  0.12866074 -0.03268525]\n",
      " [ 0.0518979  -0.03268525  0.11364792]]\n",
      "num = \n",
      "30\n",
      "SENSOR 8 4.87575158242 -0.471161323013\n",
      "\n",
      "num = \n",
      "31\n",
      "SENSOR 1 1.14340287924 -0.63651274602\n",
      "\n",
      "num = \n",
      "32\n",
      "SENSOR 2 3.52742272729 0.827358592505\n",
      "\n",
      "num = \n",
      "33\n",
      "ODOMETRY 0.0992390051029 0.100115677426 -0.000216811832096\n",
      "\n",
      "sqP = \n",
      "[[1.4393276982905887, 0.0, 0.0], [-0.034595236633799424, 0.3570208768648262, 0.0], [0.03605704065520048, -0.08805604691036754, 0.3234098638301024]]\n",
      "sigma = \n",
      "[[ 0.16540402  0.89992155  0.41123892]\n",
      " [ 3.04405941  0.89992155  0.41123892]\n",
      " [ 0.09621354  1.61396331  0.41123892]\n",
      " [ 0.2375181   0.72380946  1.05805865]\n",
      " [-2.71325138  0.89992155  0.41123892]\n",
      " [ 0.23459449  0.1858798   0.41123892]\n",
      " [ 0.09328994  1.07603365 -0.23558081]]\n",
      "sigmax = \n",
      "[[ 0.2670251   0.99625359  0.43752313]\n",
      " [-3.13750481  0.80006864  0.4108965 ]\n",
      " [ 0.19783463  1.71188201  0.4308003 ]\n",
      " [ 0.33913918  0.8179973   1.09121542]\n",
      " [-2.61163029  0.81373212  0.36081965]\n",
      " [ 0.33621558  0.28016417  0.44412018]\n",
      " [ 0.19491102  1.17400912 -0.21630579]]\n",
      "sqP = \n",
      "[[1.438864749051702, 0.0, 0.0], [0.06692850551534667, 0.5816172666804965, 0.0], [0.02826055822266666, -0.05224777597379918, 0.5513526144178833]]\n",
      "m = \n",
      "[-0.51126217  0.94199921  0.5515484 ]\n",
      "P = \n",
      "[[ 2.0666528   0.07084407  0.02860493]\n",
      " [ 0.07084407  0.12148201 -0.02840146]\n",
      " [ 0.02860493 -0.02840146  0.11335992]]\n",
      "num = \n",
      "34\n",
      "SENSOR 8 4.72075160291 -0.539321605004\n",
      "\n",
      "num = \n",
      "35\n",
      "SENSOR 1 1.21638996495 -0.860593555215\n",
      "\n",
      "num = \n",
      "36\n",
      "SENSOR 2 3.70903844372 0.752159761487\n",
      "\n",
      "num = \n",
      "37\n",
      "ODOMETRY 0.0489965903845 0.100051905877 -0.000257629385974\n",
      "\n",
      "sqP = \n",
      "[[1.4375857524850264, 0.0, 0.0], [0.04927989080246475, 0.34504131035236785, 0.0], [0.01989789557916308, -0.08515509549916665, 0.32513474976490586]]\n",
      "sigma = \n",
      "[[-0.51126217  0.94199921  0.5515484 ]\n",
      " [ 2.36390933  0.94199921  0.5515484 ]\n",
      " [-0.41270239  1.63208183  0.5515484 ]\n",
      " [-0.47146638  0.77168901  1.2018179 ]\n",
      " [-3.38643368  0.94199921  0.5515484 ]\n",
      " [-0.60982196  0.25191659  0.5515484 ]\n",
      " [-0.55105796  1.1123094  -0.0987211 ]]\n",
      "sigmax = \n",
      "[[-0.41223998  1.03373645  0.51145568]\n",
      " [ 2.46293152  0.86405415  0.61437926]\n",
      " [-0.3136802   1.727319    0.52067722]\n",
      " [-0.37244419  0.86494873  1.16540671]\n",
      " [ 2.99577382  0.84294288  0.566074  ]\n",
      " [-0.51079976  0.33926349  0.50262328]\n",
      " [-0.45203577  1.20237891 -0.14243187]]\n",
      "sqP = \n",
      "[[1.4391387411219956, 0.0, 0.0], [-0.06347409167149276, 0.5741194657270521, 0.0], [0.03576793628718836, -0.04587572537684725, 0.5521286820874461]]\n",
      "m = \n",
      "[0.36656362 1.03597154 0.50689717]\n",
      "P = \n",
      "[[ 2.06786227 -0.0673258   0.04064377]\n",
      " [-0.0673258   0.11668961 -0.02795792]\n",
      " [ 0.04064377 -0.02795792  0.11470181]]\n",
      "num = \n",
      "38\n",
      "SENSOR 8 4.86663909002 -0.673941110611\n",
      "\n",
      "num = \n",
      "39\n",
      "SENSOR 1 1.19962918841 -0.911110534625\n",
      "\n",
      "num = \n",
      "40\n",
      "SENSOR 2 3.4305673166 0.673488499054\n",
      "\n",
      "num = \n",
      "41\n",
      "ODOMETRY 0.0998222275061 0.0996351296598 -0.000153756252698\n",
      "\n",
      "sqP = \n",
      "[[1.4380063529430458, 0.0, 0.0], [-0.04681884461530685, 0.33837494456025224, 0.0], [0.02826396962667589, -0.0787133646250372, 0.32818769293280836]]\n",
      "sigma = \n",
      "[[ 0.36656362  1.03597154  0.50689717]\n",
      " [ 3.24257632  1.03597154  0.50689717]\n",
      " [ 0.27292593  1.71272143  0.50689717]\n",
      " [ 0.42309156  0.87854481  1.16327255]\n",
      " [-2.50944909  1.03597154  0.50689717]\n",
      " [ 0.46020131  0.35922165  0.50689717]\n",
      " [ 0.31003568  1.19339827 -0.14947822]]\n",
      "sigmax = \n",
      "[[ 0.41530258  1.12750806  0.54728837]\n",
      " [-2.99187002  0.93704281  0.49194755]\n",
      " [ 0.32166489  1.80763356  0.53855268]\n",
      " [ 0.47183052  0.9676531   1.20877086]\n",
      " [-2.46071013  0.95824569  0.44389657]\n",
      " [ 0.50894027  0.44658055  0.55567018]\n",
      " [ 0.35877464  1.28707059 -0.11432314]]\n",
      "sqP = \n",
      "[[1.4391576906475536, 0.0, 0.0], [0.06139510095369234, 0.5696209924091234, 0.0], [0.03797871754118847, -0.048442803760202, 0.5540427006497076]]\n",
      "m = \n",
      "[-0.37806534  0.93506304  0.6288473 ]\n",
      "P = \n",
      "[[ 2.06760981  0.06609592  0.03970751]\n",
      " [ 0.06609592  0.11956876 -0.02231911]\n",
      " [ 0.03970751 -0.02231911  0.11495111]]\n",
      "num = \n",
      "42\n",
      "SENSOR 8 4.58351837652 -0.750587339257\n",
      "\n",
      "num = \n",
      "43\n",
      "SENSOR 1 1.19033672649 -1.04401316016\n",
      "\n",
      "num = \n",
      "44\n",
      "SENSOR 2 3.21903139503 0.608609177764\n",
      "\n",
      "num = \n",
      "45\n",
      "ODOMETRY 0.101077500499 0.0998498505796 -0.000160756806792\n",
      "\n",
      "sqP = \n",
      "[[1.437918567388165, 0.0, 0.0], [0.04596638461424361, 0.342718332120684, 0.0], [0.027614574040696702, -0.06882750473421337, 0.3308342710508467]]\n",
      "sigma = \n",
      "[[-0.37806534  0.93506304  0.6288473 ]\n",
      " [ 2.49777179  0.93506304  0.6288473 ]\n",
      " [-0.28613257  1.62049971  0.6288473 ]\n",
      " [-0.32283619  0.79740803  1.29051585]\n",
      " [-3.25390248  0.93506304  0.6288473 ]\n",
      " [-0.46999811  0.24962638  0.6288473 ]\n",
      " [-0.43329449  1.07271805 -0.03282124]]\n",
      "sigmax = \n",
      "[[-0.27839687  1.03086615  0.60148085]\n",
      " [ 2.59744026  0.84981064  0.68041461]\n",
      " [-0.1864641   1.71841059  0.61039145]\n",
      " [-0.22316772  0.89457573  1.26847955]\n",
      " [ 3.1289513   0.83543568  0.63009147]\n",
      " [-0.37032964  0.3425126   0.59280137]\n",
      " [-0.33362602  1.16686443 -0.0654344 ]]\n",
      "sqP = \n",
      "[[1.4391257058933946, 0.0, 0.0], [-0.06762592395439958, 0.5701080058806529, 0.0], [0.027365819085297, -0.03716198738908444, 0.5565938438128188]]\n",
      "m = \n",
      "[0.51809132 0.96683643 0.83542924]\n",
      "P = \n",
      "[[ 2.06771163 -0.0716869   0.03137443]\n",
      " [-0.0716869   0.11318863 -0.02388248]\n",
      " [ 0.03137443 -0.02388248  0.11533618]]\n",
      "num = \n",
      "46\n",
      "SENSOR 8 4.47638974681 -0.87477261163\n",
      "\n",
      "num = \n",
      "47\n",
      "SENSOR 1 0.930451391422 -1.25663412504\n",
      "\n",
      "num = \n",
      "48\n",
      "SENSOR 2 3.30661799717 0.524107985292\n",
      "\n",
      "num = \n",
      "49\n",
      "ODOMETRY 0.100521808815 0.100140214715 -0.00102153918024\n",
      "\n",
      "sqP = \n",
      "[[1.437953973104933, 0.0, 0.0], [-0.04985340233676885, 0.33272099952937123, 0.0], [0.02181880063382614, -0.06851006985713236, 0.33191337440579965]]\n",
      "sigma = \n",
      "[[ 0.51809132  0.96683643  0.83542924]\n",
      " [ 3.39399927  0.96683643  0.83542924]\n",
      " [ 0.41838452  1.63227843  0.83542924]\n",
      " [ 0.56172892  0.82981629  1.49925599]\n",
      " [-2.35781663  0.96683643  0.83542924]\n",
      " [ 0.61779813  0.30139443  0.83542924]\n",
      " [ 0.47445372  1.10385657  0.17160249]]\n",
      "sigmax = \n",
      "[[ 0.61900806  1.04815027  0.89337795]\n",
      " [-2.7882693   0.87316006  0.80086436]\n",
      " [ 0.51930126  1.71895672  0.88499603]\n",
      " [ 0.66264567  0.90852478  1.56069675]\n",
      " [-2.25689988  0.90359125  0.75816321]\n",
      " [ 0.71871487  0.3765361   0.90118426]\n",
      " [ 0.57537046  1.18762094  0.22594882]]\n",
      "sqP = \n",
      "[[1.4391522983769975, 0.0, 0.0], [0.053349260364641694, 0.5652325841369757, 0.0], [0.0507718789179231, -0.04250723969967264, 0.556344113145454]]\n",
      "m = \n",
      "[-0.15292166  1.11436545  0.89973304]\n",
      "P = \n",
      "[[ 2.06751256  0.05696208  0.05502778]\n",
      " [ 0.05696208  0.10169105 -0.01106438]\n",
      " [ 0.05502778 -0.01106438  0.1208679 ]]\n",
      "num = \n",
      "50\n",
      "SENSOR 8 4.42544858746 -1.03059237397\n",
      "\n",
      "num = \n",
      "51\n",
      "SENSOR 1 1.09621121037 -1.43900397462\n",
      "\n",
      "num = \n",
      "52\n",
      "SENSOR 2 3.2382990193 0.518547924017\n",
      "\n",
      "num = \n",
      "53\n",
      "ODOMETRY 0.0991500821073 0.10013162298 0.00061821939171\n",
      "\n",
      "sqP = \n",
      "[[1.4378847513681674, 0.0, 0.0], [0.039615191336334475, 0.3164201137905165, 0.0], [0.038269953234185417, -0.0397587031544148, 0.34325289900245826]]\n",
      "sigma = \n",
      "[[-0.15292166  1.11436545  0.89973304]\n",
      " [ 2.72284784  1.11436545  0.89973304]\n",
      " [-0.07369128  1.74720568  0.89973304]\n",
      " [-0.07638175  1.03484805  1.58623884]\n",
      " [-3.02869116  1.11436545  0.89973304]\n",
      " [-0.23215204  0.48152522  0.89973304]\n",
      " [-0.22946156  1.19388286  0.21322724]]\n",
      "sigmax = \n",
      "[[-0.05342139  1.21436822  0.89448811]\n",
      " [ 2.82234811  1.01925299  0.93106484]\n",
      " [ 0.02580899  1.84730985  0.90241953]\n",
      " [ 0.02311852  1.13495908  1.58865599]\n",
      " [-2.92919089  1.01649726  0.87852266]\n",
      " [-0.13265177  0.58079915  0.88658959]\n",
      " [-0.12996129  1.29319179  0.20035093]]\n",
      "sqP = \n",
      "[[1.506833272436688, 0.0, 0.0], [0.007976980737251603, 0.5558589667648822, 0.0], [0.021453445964981274, -0.023418968932699066, 0.5653859848820603]]\n",
      "m = \n",
      "[-0.05157311  1.11974079  0.96191871]\n",
      "P = \n",
      "[[ 2.27021139  0.00926776  0.02465594]\n",
      " [ 0.00926776  0.1056938  -0.00853945]\n",
      " [ 0.02465594 -0.00853945  0.12002662]]\n",
      "num = \n",
      "54\n",
      "SENSOR 8 4.48032266707 -1.16853964317\n",
      "\n",
      "num = \n",
      "55\n",
      "SENSOR 1 1.05453646597 -1.57360343045\n",
      "\n",
      "num = \n",
      "56\n",
      "SENSOR 2 3.12147771546 0.369604279194\n",
      "\n",
      "num = \n",
      "57\n",
      "ODOMETRY 0.000277686359907 0.0999909476789 -5.65207074877e-05\n",
      "\n",
      "sqP = \n",
      "[[1.5067220669585701, 0.0, 0.0], [0.006150944710013769, 0.3250476385285094, 0.0], [0.01636395965004967, -0.02658104363233774, 0.34503954073484766]]\n",
      "sigma = \n",
      "[[-0.05157311  1.11974079  0.96191871]\n",
      " [ 2.96187103  1.11974079  0.96191871]\n",
      " [-0.03927122  1.76983606  0.96191871]\n",
      " [-0.01884519  1.0665787   1.65199779]\n",
      " [-3.06501724  1.11974079  0.96191871]\n",
      " [-0.063875    0.46964551  0.96191871]\n",
      " [-0.08430103  1.17290287  0.27183963]]\n",
      "sigmax = \n",
      "[[ 0.04819519  1.2197591   0.96668088]\n",
      " [ 3.06163933  1.019934    0.96997775]\n",
      " [ 0.06049708  1.86978823  0.9679109 ]\n",
      " [ 0.08092311  1.16638763  1.66003021]\n",
      " [-2.96524894  1.02115119  0.94441345]\n",
      " [ 0.0358933   0.56971484  0.96545013]\n",
      " [ 0.01546727  1.27302346  0.27332644]]\n",
      "sqP = \n",
      "[[1.57178783566872, 0.0, 0.0], [0.0007026596554653859, 0.5601669896411682, 0.0], [0.009738176180972221, -0.015525060059619176, 0.5656027237095891]]\n",
      "m = \n",
      "[0.04935875 1.08469156 1.03795296]\n",
      "P = \n",
      "[[ 2.47045733e+00  8.93874331e-04  1.18628341e-02]\n",
      " [ 8.93874331e-04  1.05578234e-01 -5.75587877e-03]\n",
      " [ 1.18628341e-02 -5.75587877e-03  1.20425575e-01]]\n",
      "num = \n",
      "58\n",
      "SENSOR 8 4.51041519435 -1.13505398802\n",
      "\n",
      "num = \n",
      "59\n",
      "SENSOR 1 1.01408592378 -1.6209817432\n",
      "\n",
      "num = \n",
      "60\n",
      "SENSOR 2 3.0349959581 0.307197125698\n",
      "\n",
      "num = \n",
      "61\n",
      "ODOMETRY -0.000206028015542 0.100097050614 0.00122219552996\n",
      "\n",
      "sqP = \n",
      "[[1.5717688537227756, 0.0, 0.0], [0.0005687059704115171, 0.3249275472937766, 0.0], [0.007547441877228684, -0.017727555259060144, 0.3464885930943963]]\n",
      "sigma = \n",
      "[[ 0.04935875  1.08469156  1.03795296]\n",
      " [ 3.19289646  1.08469156  1.03795296]\n",
      " [ 0.05049616  1.73454665  1.03795296]\n",
      " [ 0.06445364  1.04923645  1.73093014]\n",
      " [-3.09417895  1.08469156  1.03795296]\n",
      " [ 0.04822134  0.43483646  1.03795296]\n",
      " [ 0.03426387  1.12014667  0.34497577]]\n",
      "sigmax = \n",
      "[[ 0.04957992  1.18455935  1.04291411]\n",
      " [-3.09006768  0.9848336   1.03279756]\n",
      " [ 0.05071733  1.83440874  1.0430277 ]\n",
      " [ 0.0646748   1.14901798  1.73739817]\n",
      " [-3.09395779  0.9848143   1.03318606]\n",
      " [ 0.04844251  0.53470984  1.04280052]\n",
      " [ 0.03448503  1.22007797  0.34842893]]\n",
      "sqP = \n",
      "[[1.4319947829228468, 0.0, 0.0], [0.08219466233033315, 0.553728807933685, 0.0], [0.005911569719098417, -0.01131352494826635, 0.5660677977665259]]\n",
      "m = \n",
      "[-0.746449    1.05723824  1.10427189]\n",
      "P = \n",
      "[[ 2.0460617   0.08726503  0.00628119]\n",
      " [ 0.08726503  0.10802172 -0.00245855]\n",
      " [ 0.00628119 -0.00245855  0.12096804]]\n",
      "num = \n",
      "62\n",
      "SENSOR 8 4.47730873513 -1.08933708692\n",
      "\n",
      "num = \n",
      "63\n",
      "SENSOR 1 0.977458402036 -1.79104939149\n",
      "\n",
      "num = \n",
      "64\n",
      "SENSOR 2 2.7939870742 0.344183271835\n",
      "\n",
      "num = \n",
      "65\n",
      "ODOMETRY -0.000589698440506 0.100045946005 -1.26097962524e-05\n",
      "\n",
      "sqP = \n",
      "[[1.4304061323325576, 0.0, 0.0], [0.06100717282873124, 0.32295486981286464, 0.0], [0.004391190032787869, -0.008442178199771, 0.3476743937683282]]\n",
      "sigma = \n",
      "[[-0.746449    1.05723824  1.10427189]\n",
      " [ 2.11436327  1.05723824  1.10427189]\n",
      " [-0.62443465  1.70314798  1.10427189]\n",
      " [-0.73766662  1.04035388  1.79962068]\n",
      " [-3.60726126  1.05723824  1.10427189]\n",
      " [-0.86846334  0.4113285   1.10427189]\n",
      " [-0.75523138  1.07412259  0.4089231 ]]\n",
      "sigmax = \n",
      "[[-0.74543283  1.13070595  1.03628722]\n",
      " [ 2.11537943  1.00548648  1.18995255]\n",
      " [-0.62341849  1.78434403  1.04573455]\n",
      " [-0.73665045  1.11441582  1.73228385]\n",
      " [ 2.67694021  0.96780863  1.14923592]\n",
      " [-0.86744718  0.47597548  1.02785077]\n",
      " [-0.75421521  1.14699042  0.34029585]]\n",
      "sqP = \n",
      "[[1.440139364642147, 0.0, 0.0], [-0.04593499342783676, 0.5558017739744618, 0.0], [0.05471441412036374, -0.004235354782211347, 0.5670427767034875]]\n",
      "m = \n",
      "[0.06310732 1.02921693 1.25479852]\n",
      "P = \n",
      "[[ 2.07071881 -0.04878135  0.05953249]\n",
      " [-0.04878135  0.10146503 -0.0043247 ]\n",
      " [ 0.05953249 -0.0043247   0.12408661]]\n",
      "num = \n",
      "66\n",
      "SENSOR 8 4.31671160573 -1.13337841163\n",
      "\n",
      "num = \n",
      "67\n",
      "SENSOR 1 0.886564013958 -1.96817341764\n",
      "\n",
      "num = \n",
      "68\n",
      "SENSOR 2 2.8062997236 0.378866179691\n",
      "\n",
      "num = \n",
      "69\n",
      "ODOMETRY 0.000268189622775 0.099944716103 0.000225158589822\n",
      "\n",
      "sqP = \n",
      "[[1.4389992378003027, 0.0, 0.0], [-0.033899494697656726, 0.3167267739898341, 0.0], [0.041370760584524456, -0.009226402389627549, 0.3496997944423098]]\n",
      "sigma = \n",
      "[[ 0.06310732  1.02921693  1.25479852]\n",
      " [ 2.9411058   1.02921693  1.25479852]\n",
      " [-0.00469166  1.66267048  1.25479852]\n",
      " [ 0.14584885  1.01076412  1.95419811]\n",
      " [-2.81489115  1.02921693  1.25479852]\n",
      " [ 0.13090631  0.39576338  1.25479852]\n",
      " [-0.0196342   1.04766973  0.55539893]]\n",
      "sigmax = \n",
      "[[ 0.06250502  1.12906742  1.26104908]\n",
      " [ 2.94050349  0.93118669  1.27478013]\n",
      " [-0.00529397  1.76271503  1.25427014]\n",
      " [ 0.14524654  1.10975643  1.96867964]\n",
      " [-2.81549346  0.93444389  1.22274758]\n",
      " [ 0.13030401  0.49496102  1.2677993 ]\n",
      " [-0.0202365   1.14769522  0.55337575]]\n",
      "sqP = \n",
      "[[1.507839355489155, 0.0, 0.0], [-0.008162819078059897, 0.5546905113557579, 0.0], [0.022198257267468578, -0.0068616908076434, 0.5699665513332048]]\n",
      "m = \n",
      "[0.06502874 1.13042402 1.34225694]\n",
      "P = \n",
      "[[ 2.27322074 -0.00890123  0.02551035]\n",
      " [-0.00890123  0.09895856  0.00463928]\n",
      " [ 0.02551035  0.00463928  0.1233325 ]]\n",
      "num = \n",
      "70\n",
      "SENSOR 8 4.44217614349 -1.17703179109\n",
      "\n",
      "num = \n",
      "71\n",
      "SENSOR 1 1.00399386332 -1.97871776411\n",
      "\n",
      "num = \n",
      "72\n",
      "SENSOR 2 2.87352149613 0.402848100533\n",
      "\n",
      "num = \n",
      "73\n",
      "ODOMETRY -0.000740364779592 0.0999166193458 0.000304584312929\n",
      "\n",
      "sqP = \n",
      "[[1.5077203782414477, 0.0, 0.0], [-0.005903764454253697, 0.31452138924879725, 0.0], [0.016919813695270652, 0.015067865652830005, 0.35045567074822087]]\n",
      "sigma = \n",
      "[[ 0.06502874  1.13042402  1.34225694]\n",
      " [ 3.0804695   1.13042402  1.34225694]\n",
      " [ 0.05322121  1.7594668   1.34225694]\n",
      " [ 0.09886837  1.16055975  2.04316828]\n",
      " [-2.95041201  1.13042402  1.34225694]\n",
      " [ 0.07683627  0.50138124  1.34225694]\n",
      " [ 0.03118912  1.10028829  0.6413456 ]]\n",
      "sigmax = \n",
      "[[ 0.06552209  1.23015575  1.34877838]\n",
      " [ 3.08096285  1.03066431  1.34833532]\n",
      " [ 0.05371456  1.85926857  1.34760037]\n",
      " [ 0.09936172  1.26001374  2.05306023]\n",
      " [-2.94991867  1.03230534  1.32323931]\n",
      " [ 0.07732962  0.60102901  1.34995549]\n",
      " [ 0.03168246  1.20018356  0.64448907]]\n",
      "sqP = \n",
      "[[1.5727497809552375, 0.0, 0.0], [-0.0014131684383471828, 0.5536866296013062, 0.0], [0.009805187079490359, 0.010072649982434071, 0.5691030593510683]]\n",
      "m = \n",
      "[0.06517415 1.07786912 1.31022782]\n",
      "P = \n",
      "[[ 2.47348056e+00 -1.62804002e-03  1.19593464e-02]\n",
      " [-1.62804002e-03  1.03126352e-01  9.92061898e-03]\n",
      " [ 1.19593464e-02  9.92061898e-03  1.24321072e-01]]\n",
      "num = \n",
      "74\n",
      "SENSOR 8 4.41002211254 -1.2055058086\n",
      "\n",
      "num = \n",
      "75\n",
      "SENSOR 1 1.20240484656 -2.09055794649\n",
      "\n",
      "num = \n",
      "76\n",
      "SENSOR 2 2.61257298591 0.400689815362\n",
      "\n",
      "num = \n",
      "77\n",
      "ODOMETRY -0.00093907783548 0.0999937264589 -0.000488136208846\n",
      "\n",
      "sqP = \n",
      "[[1.572730287239852, 0.0, 0.0], [-0.0010351679687051524, 0.3211312505299342, 0.0], [0.007604194092678556, 0.030917235812767296, 0.3511514959417881]]\n",
      "sigma = \n",
      "[[ 0.06517415  1.07786912  1.31022782]\n",
      " [ 3.21063472  1.07786912  1.31022782]\n",
      " [ 0.06310381  1.72013162  1.31022782]\n",
      " [ 0.08038254  1.13970359  2.01253081]\n",
      " [-3.08028643  1.07786912  1.31022782]\n",
      " [ 0.06724448  0.43560662  1.31022782]\n",
      " [ 0.04996576  1.01603465  0.60792483]]\n",
      "sigmax = \n",
      "[[ 0.06473837  1.1775784   1.31666137]\n",
      " [-3.07298636  0.97818547  1.30340865]\n",
      " [ 0.06266803  1.819854    1.31645493]\n",
      " [ 0.07994676  1.2393035   2.02047998]\n",
      " [-3.08072221  0.9781357   1.30417998]\n",
      " [ 0.0668087   0.53530236  1.31686779]\n",
      " [ 0.04952998  1.11583024  0.61284128]]\n",
      "sqP = \n",
      "[[1.4319963200887702, 0.0, 0.0], [0.08196026790400603, 0.5521059967688233, 0.0], [0.007161258676355223, 0.01942355739860379, 0.5687204504650497]]\n",
      "m = \n",
      "[-0.74056689  0.98570149  1.46975419]\n",
      "P = \n",
      "[[2.04609415 0.0870127  0.00849447]\n",
      " [0.0870127  0.10715336 0.00957266]\n",
      " [0.00849447 0.00957266 0.1241338 ]]\n",
      "num = \n",
      "78\n",
      "SENSOR 8 4.28220992404 -1.22818070834\n",
      "\n",
      "num = \n",
      "79\n",
      "SENSOR 1 1.35806022311 -2.03308175218\n",
      "\n",
      "num = \n",
      "80\n",
      "SENSOR 2 2.41796456399 0.428960172356\n",
      "\n",
      "num = \n",
      "81\n",
      "ODOMETRY 0.000672986065937 0.0999170760041 9.59803128141e-05\n",
      "\n",
      "sqP = \n",
      "[[1.430417472535161, 0.0, 0.0], [0.06083028494017576, 0.3216411625314788, 0.0], [0.00593845674353448, 0.028638810594215726, 0.35111016643598764]]\n",
      "sigma = \n",
      "[[-0.74056689  0.98570149  1.46975419]\n",
      " [ 2.12026806  0.98570149  1.46975419]\n",
      " [-0.61890632  1.62898382  1.46975419]\n",
      " [-0.72868998  1.04297911  2.17197453]\n",
      " [-3.60140183  0.98570149  1.46975419]\n",
      " [-0.86222746  0.34241917  1.46975419]\n",
      " [-0.7524438   0.92842387  0.76753386]]\n",
      "sigmax = \n",
      "[[-0.7419941   1.05944209  1.4022185 ]\n",
      " [ 2.11884084  0.93356119  1.5550779 ]\n",
      " [-0.62033353  1.71037554  1.4116669 ]\n",
      " [-0.73011719  1.11751661  2.10531939]\n",
      " [ 2.68035626  0.8961351   1.51421325]\n",
      " [-0.86365467  0.40741853  1.39376849]\n",
      " [-0.75387102  1.00135717  0.69912714]]\n",
      "sqP = \n",
      "[[1.440136325755978, 0.0, 0.0], [-0.04610050726147869, 0.5557174728032839, 0.0], [0.054739777777197136, 0.019262232227892126, 0.5688785750068488]]\n",
      "m = \n",
      "[0.07238995 0.9551806  1.67119978]\n",
      "P = \n",
      "[[ 2.07055429 -0.04878876  0.05885835]\n",
      " [-0.04878876  0.10054456  0.00967469]\n",
      " [ 0.05885835  0.00967469  0.12654722]]\n",
      "num = \n",
      "82\n",
      "SENSOR 8 4.22916152226 -1.1920443901\n",
      "\n",
      "num = \n",
      "83\n",
      "SENSOR 1 1.43456173466 -2.20151685421\n",
      "\n",
      "num = \n",
      "84\n",
      "SENSOR 2 2.54268409924 0.418576167122\n",
      "\n",
      "num = \n",
      "85\n",
      "ODOMETRY -0.000190086120117 0.0998500295775 -0.00120829073569\n",
      "\n",
      "sqP = \n",
      "[[1.4389420727484465, 0.0, 0.0], [-0.0339059933181114, 0.31526963265940494, 0.0], [0.040903905679723296, 0.03508607868388105, 0.35162914467305945]]\n",
      "sigma = \n",
      "[[ 0.07238995  0.9551806   1.67119978]\n",
      " [ 2.95027409  0.9551806   1.67119978]\n",
      " [ 0.00457796  1.58571986  1.67119978]\n",
      " [ 0.15419776  1.02535276  2.37445807]\n",
      " [-2.8054942   0.9551806   1.67119978]\n",
      " [ 0.14020193  0.32464133  1.67119978]\n",
      " [-0.00941786  0.88500844  0.9679415 ]]\n",
      "sigmax = \n",
      "[[ 0.07315891  1.05483111  1.67849353]\n",
      " [ 2.95104306  0.85707381  1.69013335]\n",
      " [ 0.00534693  1.68563556  1.67172444]\n",
      " [ 0.15496672  1.12407397  2.38987052]\n",
      " [-2.80472523  0.86087622  1.63818302]\n",
      " [ 0.1409709   0.42356858  1.68522908]\n",
      " [-0.0086489   0.9849217   0.96706774]]\n",
      "sqP = \n",
      "[[1.5077722091245545, 0.0, 0.0], [-0.007058628122230059, 0.5548790738916644, 0.0], [0.02212031618936048, 0.02158691774774948, 0.5707768516822588]]\n",
      "m = \n",
      "[0.07521011 0.98980538 1.72201598]\n",
      "P = \n",
      "[[ 2.27302374 -0.00769592  0.02530348]\n",
      " [-0.00769592  0.10059204  0.01970236]\n",
      " [ 0.02530348  0.01970236  0.12415458]]\n",
      "num = \n",
      "86\n",
      "SENSOR 8 4.25479049036 -1.29393177827\n",
      "\n",
      "num = \n",
      "87\n",
      "SENSOR 1 1.3915222382 -2.32656801851\n",
      "\n",
      "num = \n",
      "88\n",
      "SENSOR 2 2.22979301015 0.456482047365\n",
      "\n",
      "num = \n",
      "89\n",
      "ODOMETRY 0.000473163132143 0.09995123511 -0.000781756651883\n",
      "\n",
      "sqP = \n",
      "[[1.5076550470689731, 0.0, 0.0], [-0.005104566002032756, 0.3171214046743353, 0.0], [0.01678333370887816, 0.06239892021271331, 0.3463802452078478]]\n",
      "sigma = \n",
      "[[ 0.07521011  0.98980538  1.72201598]\n",
      " [ 3.0905202   0.98980538  1.72201598]\n",
      " [ 0.06500097  1.62404819  1.72201598]\n",
      " [ 0.10877677  1.11460322  2.41477647]\n",
      " [-2.94009999  0.98980538  1.72201598]\n",
      " [ 0.08541924  0.35556257  1.72201598]\n",
      " [ 0.04164344  0.86500754  1.02925549]]\n",
      "sigmax = \n",
      "[[ 0.07381173  1.08937456  1.72949971]\n",
      " [ 3.08912182  0.89008651  1.72713231]\n",
      " [ 0.0636026   1.72368858  1.72848282]\n",
      " [ 0.1073784   1.21386516  2.42559756]\n",
      " [-2.94149836  0.89197162  1.70205139]\n",
      " [ 0.08402086  0.45505016  1.73051582]\n",
      " [ 0.04024506  0.96477179  1.03339343]]\n",
      "sqP = \n",
      "[[1.5726828916971787, 0.0, 0.0], [-0.000816649849054415, 0.5584287294888386, 0.0], [0.009726940900799107, 0.039525902489250635, 0.5652997863144785]]\n",
      "m = \n",
      "[0.07659287 0.96935188 1.88049836]\n",
      "P = \n",
      "[[ 2.47327210e+00 -9.59384000e-04  1.18944600e-02]\n",
      " [-9.59384000e-04  1.01603889e-01  2.30750160e-02]\n",
      " [ 1.18944600e-02  2.30750160e-02  1.24706449e-01]]\n",
      "num = \n",
      "90\n",
      "SENSOR 8 4.18161649136 -1.28816567381\n",
      "\n",
      "num = \n",
      "91\n",
      "SENSOR 1 1.3025278855 -2.3472367671\n",
      "\n",
      "num = \n",
      "92\n",
      "SENSOR 2 2.30576040901 0.369157306239\n",
      "\n",
      "num = \n",
      "93\n",
      "ODOMETRY -0.000559278187115 0.100028723409 9.13999205286e-06\n",
      "\n",
      "sqP = \n",
      "[[1.5726640141609238, 0.0, 0.0], [-0.0006100374847291469, 0.3187530655093204, 0.0], [0.007563255651682097, 0.07240598566798338, 0.3455526292042498]]\n",
      "sigma = \n",
      "[[ 0.07659287  0.96935188  1.88049836]\n",
      " [ 3.2219209   0.96935188  1.88049836]\n",
      " [ 0.0753728   1.60685801  1.88049836]\n",
      " [ 0.09171938  1.11416385  2.57160362]\n",
      " [-3.06873516  0.96935188  1.88049836]\n",
      " [ 0.07781295  0.33184575  1.88049836]\n",
      " [ 0.06146636  0.82453991  1.1893931 ]]\n",
      "sigmax = \n",
      "[[ 0.07628428  1.06900645  1.88819358]\n",
      " [-3.061573    0.86972675  1.87243094]\n",
      " [ 0.0750642   1.70652189  1.88807199]\n",
      " [ 0.09141079  1.21369062  2.58080533]\n",
      " [-3.06904375  0.86966926  1.87317543]\n",
      " [ 0.07750435  0.43149085  1.88831516]\n",
      " [ 0.06115777  0.92429947  1.19558007]]\n",
      "sqP = \n",
      "[[1.431995771097854, 0.0, 0.0], [0.08222989855358971, 0.5545510730676901, 0.0], [0.00815952674509958, 0.04497201540390243, 0.5638115388528498]]\n",
      "m = \n",
      "[-0.70633807  1.03771406  1.89456724]\n",
      "P = \n",
      "[[2.04609392 0.08728662 0.01154094]\n",
      " [0.08728662 0.10878217 0.02812524]\n",
      " [0.01154094 0.02812524 0.12470031]]\n",
      "num = \n",
      "94\n",
      "SENSOR 8 4.17489666144 -1.34658482855\n",
      "\n",
      "num = \n",
      "95\n",
      "SENSOR 1 1.3978512621 -2.36482377797\n",
      "\n",
      "num = \n",
      "96\n",
      "SENSOR 2 2.11259117923 0.471244681669\n",
      "\n",
      "num = \n",
      "97\n",
      "SENSOR 7 5.05551177517 -0.919442363958\n",
      "\n",
      "num = \n",
      "98\n",
      "ODOMETRY 0.000257701615542 0.10004347626 -0.000159471500187\n",
      "\n",
      "sqP = \n",
      "[[1.4304173921681156, 0.0, 0.0], [0.061021785104879045, 0.3241273118085135, 0.0], [0.008068234356523355, 0.08525322219010908, 0.34258881867815066]]\n",
      "sigma = \n",
      "[[-0.70633807  1.03771406  1.89456724]\n",
      " [ 2.15449672  1.03771406  1.89456724]\n",
      " [-0.5842945   1.68596868  1.89456724]\n",
      " [-0.6902016   1.2082205   2.57974488]\n",
      " [-3.56717285  1.03771406  1.89456724]\n",
      " [-0.82838164  0.38945943  1.89456724]\n",
      " [-0.72247454  0.86720761  1.2093896 ]]\n",
      "sigmax = \n",
      "[[-0.7068882   1.11377396  1.82960081]\n",
      " [ 2.15394658  0.98263338  1.97806493]\n",
      " [-0.58484463  1.76937191  1.83934364]\n",
      " [-0.69075174  1.28531879  2.51601419]\n",
      " [ 2.71546232  0.94663107  1.93591497]\n",
      " [-0.82893177  0.45704453  1.82082444]\n",
      " [-0.72302467  0.94220933  1.14320435]]\n",
      "sqP = \n",
      "[[1.4401547920811228, 0.0, 0.0], [-0.04741850562396151, 0.5630167738240981, 0.0], [0.053129128536045574, 0.053317946980447256, 0.5615067744314776]]\n",
      "m = \n",
      "[0.09817605 1.00114864 1.99150098]\n",
      "P = \n",
      "[[ 2.07052301 -0.05097282  0.05639723]\n",
      " [-0.05097282  0.07692479 -0.00262086]\n",
      " [ 0.05639723 -0.00262086  0.10019343]]\n",
      "num = \n",
      "99\n",
      "SENSOR 8 3.962669916 -1.30912369209\n",
      "\n",
      "num = \n",
      "100\n",
      "SENSOR 1 1.70916168099 -2.46045075948\n",
      "\n",
      "num = \n",
      "101\n",
      "SENSOR 2 2.02595774502 0.554005367169\n",
      "\n",
      "num = \n",
      "102\n",
      "SENSOR 3 4.7371187823 -0.173423905805\n",
      "\n",
      "num = \n",
      "103\n",
      "SENSOR 7 4.82701773051 -0.911583275309\n",
      "\n",
      "num = \n",
      "104\n",
      "ODOMETRY -0.000132246838171 0.100204785792 0.000451918927672\n",
      "\n",
      "sqP = \n",
      "[[1.4389312047264147, 0.0, 0.0], [-0.03542408348052721, 0.27508166772046017, 0.0], [0.039193834862194896, -0.004480304959639414, 0.3140656017368819]]\n",
      "sigma = \n",
      "[[ 0.09817605  1.00114864  1.99150098]\n",
      " [ 2.97603845  1.00114864  1.99150098]\n",
      " [ 0.02732788  1.55131198  1.99150098]\n",
      " [ 0.17656371  0.99218803  2.61963218]\n",
      " [-2.77968636  1.00114864  1.99150098]\n",
      " [ 0.16902421  0.4509853   1.99150098]\n",
      " [ 0.01978838  1.01010925  1.36336977]]\n",
      "sigmax = \n",
      "[[ 0.09827428  1.10070784  2.00133274]\n",
      " [ 2.97613668  0.9024688   2.00796261]\n",
      " [ 0.02742611  1.65131739  1.99426038]\n",
      " [ 0.17666194  1.09067161  2.63722997]\n",
      " [-2.77958813  0.90759475  1.95605571]\n",
      " [ 0.16912244  0.54959876  2.00835576]\n",
      " [ 0.01988661  1.11013263  1.36537512]]\n",
      "sqP = \n",
      "[[1.5077513171332502, 0.0, 0.0], [-0.007820590846674238, 0.5319692572338962, 0.0], [0.02073263812410948, -0.003128762653007474, 0.548554450248969]]\n",
      "m = \n",
      "[0.10871264 1.06785471 2.27960286]\n",
      "P = \n",
      "[[ 2.2729339  -0.0087803   0.02235348]\n",
      " [-0.0087803   0.07309265 -0.00382745]\n",
      " [ 0.02235348 -0.00382745  0.06440341]]\n",
      "num = \n",
      "105\n",
      "SENSOR 8 3.98259516004 -1.43618538277\n",
      "\n",
      "num = \n",
      "106\n",
      "SENSOR 1 1.79511416601 -2.47746949194\n",
      "\n",
      "num = \n",
      "107\n",
      "SENSOR 2 2.06053687043 0.463761690139\n",
      "\n",
      "num = \n",
      "108\n",
      "SENSOR 3 4.8224768036 -0.239591007404\n",
      "\n",
      "num = \n",
      "109\n",
      "SENSOR 7 4.77817974323 -1.00970624708\n",
      "\n",
      "num = \n",
      "110\n",
      "ODOMETRY 0.000547071844751 0.100153178582 -0.00035008972123\n",
      "\n",
      "sqP = \n",
      "[[1.5076252528938192, 0.0, 0.0], [-0.005823927189256267, 0.27029379024932776, 0.0], [0.014826946560032769, -0.013840850161031617, 0.25296640531225145]]\n",
      "sigma = \n",
      "[[ 0.10871264  1.06785471  2.27960286]\n",
      " [ 3.12396315  1.06785471  2.27960286]\n",
      " [ 0.09706479  1.60844229  2.27960286]\n",
      " [ 0.13836653  1.04017301  2.78553568]\n",
      " [-2.90653787  1.06785471  2.27960286]\n",
      " [ 0.12036049  0.52726713  2.27960286]\n",
      " [ 0.07905875  1.09553641  1.77367005]]\n",
      "sigmax = \n",
      "[[ 0.10903231  1.16746939  2.29046177]\n",
      " [ 3.12428282  0.96766573  2.28138258]\n",
      " [ 0.09738446  1.70817669  2.28930077]\n",
      " [ 0.1386862   1.13942193  2.79934334]\n",
      " [-2.90621819  0.97040232  2.25627843]\n",
      " [ 0.12068017  0.62674857  2.29162131]\n",
      " [ 0.07937842  1.19542926  1.78157066]]\n",
      "sqP = \n",
      "[[1.572637167192532, 0.0, 0.0], [-0.0017890833460376934, 0.5297811812189414, 0.0], [0.008417648834737635, -0.005481617870640122, 0.5145559888988877]]\n",
      "m = \n",
      "[0.10991247 1.08019018 2.33118282]\n",
      "P = \n",
      "[[ 2.47313074e+00 -2.07284161e-03  9.90389668e-03]\n",
      " [-2.07284161e-03  7.09300990e-02 -9.31964835e-04]\n",
      " [ 9.90389668e-03 -9.31964835e-04  6.10285280e-02]]\n",
      "num = \n",
      "111\n",
      "SENSOR 8 3.83264320308 -1.36158444573\n",
      "\n",
      "num = \n",
      "112\n",
      "SENSOR 1 1.80852245484 -2.51982496643\n",
      "\n",
      "num = \n",
      "113\n",
      "SENSOR 2 1.96102088098 0.610994481673\n",
      "\n",
      "num = \n",
      "114\n",
      "SENSOR 3 4.85211156128 -0.1489018205\n",
      "\n",
      "num = \n",
      "115\n",
      "SENSOR 7 4.71279477962 -0.937135174382\n",
      "\n",
      "num = \n",
      "116\n",
      "ODOMETRY -0.000479308539659 0.0998432674256 -0.000777162798183\n",
      "\n",
      "sqP = \n",
      "[[1.5726190692066573, 0.0, 0.0], [-0.0013180824560886336, 0.26632379092973846, 0.0], [0.006297708626532087, -0.003468199113632879, 0.24693488715899858]]\n",
      "sigma = \n",
      "[[ 0.10991247  1.08019018  2.33118282]\n",
      " [ 3.25515061  1.08019018  2.33118282]\n",
      " [ 0.10727631  1.61283776  2.33118282]\n",
      " [ 0.12250789  1.07325378  2.82505259]\n",
      " [-3.03532567  1.08019018  2.33118282]\n",
      " [ 0.11254863  0.54754259  2.33118282]\n",
      " [ 0.09731705  1.08712657  1.83731304]]\n",
      "sigmax = \n",
      "[[ 0.11010945  1.17973298  2.34222321]\n",
      " [-3.02783772  0.98068829  2.31977962]\n",
      " [ 0.10747329  1.71240932  2.34196076]\n",
      " [ 0.12270487  1.17264963  2.83734586]\n",
      " [-3.03512869  0.98060779  2.32050538]\n",
      " [ 0.11274562  0.64705594  2.34248558]\n",
      " [ 0.09751403  1.18680053  1.84709881]]\n",
      "sqP = \n",
      "[[1.4319900649339226, 0.0, 0.0], [0.08163170550994935, 0.5212640557254248, 0.0], [0.010171401306882507, -0.0017588935316682707, 0.5111474734743138]]\n",
      "m = \n",
      "[-0.67443859  1.129815    2.37940626]\n",
      "P = \n",
      "[[2.04556637e+00 8.53045966e-02 1.05587202e-02]\n",
      " [8.53045966e-02 7.65764656e-02 4.96216130e-04]\n",
      " [1.05587202e-02 4.96216130e-04 6.08753154e-02]]\n",
      "num = \n",
      "117\n",
      "SENSOR 8 3.98024682642 -1.42089195757\n",
      "\n",
      "num = \n",
      "118\n",
      "SENSOR 1 1.68616417487 -2.51982610203\n",
      "\n",
      "num = \n",
      "119\n",
      "SENSOR 2 1.84497621719 0.568224090854\n",
      "\n",
      "num = \n",
      "120\n",
      "SENSOR 3 4.61852185259 -0.195327764501\n",
      "\n",
      "num = \n",
      "121\n",
      "SENSOR 7 4.6199153731 -0.979883951883\n",
      "\n",
      "num = \n",
      "122\n",
      "ODOMETRY 5.14521975978e-05 0.0998424550095 -0.00128773957906\n",
      "\n",
      "sqP = \n",
      "[[1.4302329779138598, 0.0, 0.0], [0.059643846797953724, 0.2702204233039727, 0.0], [0.007382517666889377, 0.00020684734630822837, 0.24661867545833782]]\n",
      "sigma = \n",
      "[[-0.67443859  1.129815    2.37940626]\n",
      " [ 2.18602736  1.129815    2.37940626]\n",
      " [-0.5551509   1.67025585  2.37940626]\n",
      " [-0.65967356  1.1302287   2.87264361]\n",
      " [-3.53490455  1.129815    2.37940626]\n",
      " [-0.79372629  0.58937416  2.37940626]\n",
      " [-0.68920363  1.12940131  1.88616891]]\n",
      "sigmax = \n",
      "[[-0.67569506  1.20776846  2.31702087]\n",
      " [ 2.18477089  1.07222983  2.46096989]\n",
      " [-0.55640737  1.75507951  2.32674105]\n",
      " [-0.66093003  1.20909475  2.81141596]\n",
      " [ 2.74702429  1.03761364  2.41771534]\n",
      " [-0.79498276  0.65934947  2.30818735]\n",
      " [-0.6904601   1.20642518  1.82263938]]\n",
      "sqP = \n",
      "[[1.440111374262722, 0.0, 0.0], [-0.05193823376048791, 0.526105014488714, 0.0], [0.050375715172977495, 0.0012327635082292358, 0.5113734515554966]]\n",
      "m = \n",
      "[0.12478101 1.12629769 2.45729672]\n",
      "P = \n",
      "[[ 2.06996613 -0.05450194  0.05278851]\n",
      " [-0.05450194  0.07311558 -0.00276023]\n",
      " [ 0.05278851 -0.00276023  0.06316471]]\n",
      "num = \n",
      "123\n",
      "SENSOR 8 4.07638490671 -1.48596910761\n",
      "\n",
      "num = \n",
      "124\n",
      "SENSOR 1 2.10918785502 -2.53013604019\n",
      "\n",
      "num = \n",
      "125\n",
      "SENSOR 2 1.87374300045 0.596795933857\n",
      "\n",
      "num = \n",
      "126\n",
      "SENSOR 3 4.63925938223 -0.116829211894\n",
      "\n",
      "num = \n",
      "127\n",
      "SENSOR 7 4.74670921432 -1.04281914214\n",
      "\n",
      "num = \n",
      "128\n",
      "ODOMETRY 0.000828058608643 0.0998754656366 0.000718626272025\n",
      "\n",
      "sqP = \n",
      "[[1.4387376846776923, 0.0, 0.0], [-0.03788177670589077, 0.2677322464691474, 0.0], [0.036690849630438796, -0.005118217580865281, 0.2485805675156959]]\n",
      "sigma = \n",
      "[[ 0.12478101  1.12629769  2.45729672]\n",
      " [ 3.00225638  1.12629769  2.45729672]\n",
      " [ 0.04901745  1.66176218  2.45729672]\n",
      " [ 0.19816271  1.11606125  2.95445786]\n",
      " [-2.75269436  1.12629769  2.45729672]\n",
      " [ 0.20054456  0.59083319  2.45729672]\n",
      " [ 0.05139931  1.13653412  1.96013559]]\n",
      "sigmax = \n",
      "[[ 0.12354472  1.22536322  2.46972796]\n",
      " [ 3.00102009  1.02742215  2.47115834]\n",
      " [ 0.04778117  1.76148446  2.46219392]\n",
      " [ 0.19692642  1.21394877  2.97411871]\n",
      " [-2.75393065  1.03391268  2.41943479]\n",
      " [ 0.19930827  0.68867361  2.47719068]\n",
      " [ 0.05016302  1.23624446  1.96527029]]\n",
      "sqP = \n",
      "[[1.507563389302329, 0.0, 0.0], [-0.008423558862641232, 0.5281945811588132, 0.0], [0.018573056052587605, -0.002623435160854849, 0.5133732326456129]]\n",
      "m = \n",
      "[0.12955724 1.00269814 2.55335933]\n",
      "P = \n",
      "[[ 2.27242830e+00 -9.29684491e-03  2.06638521e-02]\n",
      " [-9.29684491e-03  6.96935473e-02  2.10236874e-04]\n",
      " [ 2.06638521e-02  2.10236874e-04  6.14420275e-02]]\n",
      "num = \n",
      "129\n",
      "SENSOR 8 3.98430154528 -1.48232363932\n",
      "\n",
      "num = \n",
      "130\n",
      "SENSOR 1 1.95702304621 -2.53750111924\n",
      "\n",
      "num = \n",
      "131\n",
      "SENSOR 2 1.58489922833 0.698949835685\n",
      "\n",
      "num = \n",
      "132\n",
      "SENSOR 3 4.4671595916 -0.168854291814\n",
      "\n",
      "num = \n",
      "133\n",
      "SENSOR 7 4.52930108719 -1.12389445264\n",
      "\n",
      "num = \n",
      "134\n",
      "ODOMETRY -0.000158830047539 0.100032780056 -0.000437720533696\n",
      "\n",
      "sqP = \n",
      "[[1.5074575616367445, 0.0, 0.0], [-0.006167234917782393, 0.2639233079438974, 0.0], [0.013707750494258408, 0.0011168994277758193, 0.24749318691447378]]\n",
      "sigma = \n",
      "[[ 0.12955724  1.00269814  2.55335933]\n",
      " [ 3.14447236  1.00269814  2.55335933]\n",
      " [ 0.11722277  1.53054476  2.55335933]\n",
      " [ 0.15697274  1.00493194  3.0483457 ]\n",
      " [-2.88535788  1.00269814  2.55335933]\n",
      " [ 0.14189171  0.47485152  2.55335933]\n",
      " [ 0.10214174  1.00046434  2.05837295]]\n",
      "sigmax = \n",
      "[[ 0.13110393  1.10172585  2.56634476]\n",
      " [-3.13716626  0.90282336  2.55298901]\n",
      " [ 0.11876946  1.6297251   2.56512234]\n",
      " [ 0.15851943  1.10356648  3.0640408 ]\n",
      " [-2.8838112   0.90610449  2.52796689]\n",
      " [ 0.1434384   0.57371154  2.56756519]\n",
      " [ 0.10368843  1.0998108   2.06863895]]\n",
      "sqP = \n",
      "[[1.4334533691704654, 0.0, 0.0], [0.07996404773038022, 0.5201923128371303, 0.0], [0.01273454843895966, 1.0696586776094706e-05, 0.5117903134329232]]\n",
      "m = \n",
      "[-0.65170484  1.04790211  2.69174151]\n",
      "P = \n",
      "[[2.04988032e+00 8.34765566e-02 1.34255825e-02]\n",
      " [8.34765566e-02 7.42168527e-02 1.94046437e-03]\n",
      " [1.34255825e-02 1.94046437e-03 6.23379098e-02]]\n",
      "num = \n",
      "135\n",
      "SENSOR 8 3.84481274007 -1.42332717446\n",
      "\n",
      "num = \n",
      "136\n",
      "SENSOR 1 1.79980984001 -2.58723622781\n",
      "\n",
      "num = \n",
      "137\n",
      "SENSOR 2 1.44542795818 0.621521479992\n",
      "\n",
      "num = \n",
      "138\n",
      "SENSOR 3 4.16016807253 -0.216426375373\n",
      "\n",
      "num = \n",
      "139\n",
      "SENSOR 7 4.49513851102 -1.01028497886\n",
      "\n",
      "num = \n",
      "140\n",
      "ODOMETRY 0.000133544385368 0.100021589888 0.00109303965336\n",
      "\n",
      "sqP = \n",
      "[[1.431740312457669, 0.0, 0.0], [0.05830425799295939, 0.26611551285608004, 0.0], [0.009377107250614103, 0.0052373462721380356, 0.24944448258611712]]\n",
      "sigma = \n",
      "[[-0.65170484  1.04790211  2.69174151]\n",
      " [ 2.21177578  1.04790211  2.69174151]\n",
      " [-0.53509633  1.58013313  2.69174151]\n",
      " [-0.63295063  1.0583768   3.19063048]\n",
      " [-3.51518547  1.04790211  2.69174151]\n",
      " [-0.76831336  0.51567108  2.69174151]\n",
      " [-0.67045906  1.03742742  2.19285255]]\n",
      "sigmax = \n",
      "[[-0.65230139  1.12742362  2.63105473]\n",
      " [ 2.21117923  0.98809716  2.77192833]\n",
      " [-0.53569288  1.66617518  2.64071874]\n",
      " [-0.63354718  1.1390224   3.13144564]\n",
      " [ 2.76740329  0.95477518  2.72826455]\n",
      " [-0.76890991  0.58759199  2.62221497]\n",
      " [-0.67105561  1.11579688  2.13068516]]\n",
      "sqP = \n",
      "[[1.4399217746003108, 0.0, 0.0], [-0.05358476624653495, 0.523919991371454, 0.0], [0.04940281078710138, 0.0037500974778444567, 0.5127687021352838]]\n",
      "m = \n",
      "[0.14302084 1.12918689 2.80621467]\n",
      "P = \n",
      "[[ 2.06932895e+00 -5.60102805e-02  5.17438925e-02]\n",
      " [-5.60102805e-02  7.02383719e-02 -1.05805764e-03]\n",
      " [ 5.17438925e-02 -1.05805764e-03  6.52622219e-02]]\n",
      "num = \n",
      "141\n",
      "SENSOR 8 4.07172862971 -1.54542949541\n",
      "\n",
      "num = \n",
      "142\n",
      "SENSOR 1 2.04327443852 -2.64266309511\n",
      "\n",
      "num = \n",
      "143\n",
      "SENSOR 2 1.62012370037 0.749575235679\n",
      "\n",
      "num = \n",
      "144\n",
      "SENSOR 3 4.2939412228 -0.211337985351\n",
      "\n",
      "num = \n",
      "145\n",
      "SENSOR 7 4.55765990908 -1.01012265001\n",
      "\n",
      "num = \n",
      "146\n",
      "ODOMETRY 0.000256984803676 0.100174110326 -7.36342307491e-05\n",
      "\n",
      "sqP = \n",
      "[[1.438516233177764, 0.0, 0.0], [-0.03893614766252311, 0.2621494769546823, 0.0], [0.035970322289382, 0.0013064612710766733, 0.25291629237554586]]\n",
      "sigma = \n",
      "[[ 0.14302084  1.12918689  2.80621467]\n",
      " [ 3.02005331  1.12918689  2.80621467]\n",
      " [ 0.06514854  1.65348584  2.80621467]\n",
      " [ 0.21496148  1.13179981  3.31204725]\n",
      " [-2.73401163  1.12918689  2.80621467]\n",
      " [ 0.22089313  0.60488793  2.80621467]\n",
      " [ 0.07108019  1.12657396  2.30038208]]\n",
      "sigmax = \n",
      "[[ 0.14424742  1.22818535  2.82048434]\n",
      " [ 3.02127989  1.02990152  2.81832806]\n",
      " [ 0.06637513  1.75329437  2.81273965]\n",
      " [ 0.21618807  1.22951651  3.33339589]\n",
      " [-2.73278504  1.03736412  2.76655488]\n",
      " [ 0.22211972  0.70247629  2.82814254]\n",
      " [ 0.07230678  1.22634204  2.30749897]]\n",
      "sqP = \n",
      "[[1.5073615494619539, 0.0, 0.0], [-0.008547335548495882, 0.5253838774178347, 0.0], [0.018571877029066756, 0.0006615178451555021, 0.5155222636357556]]\n",
      "m = \n",
      "[0.14638155 1.04669521 2.81276154]\n",
      "P = \n",
      "[[ 2.27181892e+00 -9.40819429e-03  2.06878925e-02]\n",
      " [-9.40819429e-03  6.62388471e-02  1.89585828e-03]\n",
      " [ 2.06878925e-02  1.89585828e-03  6.41771090e-02]]\n",
      "num = \n",
      "147\n",
      "SENSOR 8 4.06698092589 -1.56722255044\n",
      "\n",
      "num = \n",
      "148\n",
      "SENSOR 1 2.24512586768 -2.5437818487\n",
      "\n",
      "num = \n",
      "149\n",
      "SENSOR 2 1.37145664293 0.831882746959\n",
      "\n",
      "num = \n",
      "150\n",
      "SENSOR 3 4.22397677315 -0.235036227202\n",
      "\n",
      "num = \n",
      "151\n",
      "SENSOR 7 4.41926718082 -1.00471126329\n",
      "\n",
      "num = \n",
      "152\n",
      "ODOMETRY 0.00058192498751 0.0999391933134 -0.00023129046615\n",
      "\n",
      "sqP = \n",
      "[[1.507255425631922, 0.0, 0.0], [-0.006241937583233548, 0.257293383709881, 0.0], [0.013725538552823715, 0.007701450400740772, 0.252842651198712]]\n",
      "sigma = \n",
      "[[ 0.14638155  1.04669521  2.81276154]\n",
      " [ 3.1608924   1.04669521  2.81276154]\n",
      " [ 0.13389768  1.56128198  2.81276154]\n",
      " [ 0.17383263  1.06209812  3.31844684]\n",
      " [-2.8681293   1.04669521  2.81276154]\n",
      " [ 0.15886543  0.53210845  2.81276154]\n",
      " [ 0.11893047  1.03129231  2.30707623]]\n",
      "sigmax = \n",
      "[[ 0.1465649   1.14579424  2.82739833]\n",
      " [-3.12210955  0.94654026  2.81080258]\n",
      " [ 0.13408103  1.660556    2.82616008]\n",
      " [ 0.17401598  1.16075806  3.33579815]\n",
      " [-2.86794595  0.9502504   2.78568296]\n",
      " [ 0.15904878  0.63101703  2.8286343 ]\n",
      " [ 0.11911382  1.13075575  2.31898748]]\n",
      "sqP = \n",
      "[[1.4334628085577301, 0.0, 0.0], [0.08010254233038422, 0.5169068417491415, 0.0], [0.014138236570246425, 0.0032657280467720665, 0.5144002536920557]]\n",
      "m = \n",
      "[-0.64956113  1.00276979  2.96299634]\n",
      "P = \n",
      "[[2.0498359  0.08358295 0.01504238]\n",
      " [0.08358295 0.07191437 0.00337098]\n",
      " [0.01504238 0.00337098 0.06446597]]\n",
      "num = \n",
      "153\n",
      "SENSOR 8 3.97991042562 -1.60690152056\n",
      "\n",
      "num = \n",
      "154\n",
      "SENSOR 1 2.35643859208 -2.67866850272\n",
      "\n",
      "num = \n",
      "155\n",
      "SENSOR 2 1.45749408492 0.830132685583\n",
      "\n",
      "num = \n",
      "156\n",
      "SENSOR 3 4.02815458891 -0.258831937496\n",
      "\n",
      "num = \n",
      "157\n",
      "SENSOR 7 4.42145324114 -1.10938279886\n",
      "\n",
      "num = \n",
      "158\n",
      "ODOMETRY 0.000696732375949 0.100156157706 -0.000584089998119\n",
      "\n",
      "sqP = \n",
      "[[1.4317247985179593, 0.0, 0.0], [0.05837920016303144, 0.2617369626668182, 0.0], [0.010506473415263482, 0.01053585603795792, 0.25346513647196134]]\n",
      "sigma = \n",
      "[[-0.64956113  1.00276979  2.96299634]\n",
      " [ 2.21388847  1.00276979  2.96299634]\n",
      " [-0.53280273  1.52624372  2.96299634]\n",
      " [-0.62854818  1.0238415   3.46992661]\n",
      " [-3.51301073  1.00276979  2.96299634]\n",
      " [-0.76631953  0.47929587  2.96299634]\n",
      " [-0.67057408  0.98169808  2.45606607]]\n",
      "sigmax = \n",
      "[[-0.6492105   1.08239146  2.90259574]\n",
      " [ 2.2142391   0.94279245  3.04293732]\n",
      " [-0.5324521   1.61235955  2.91228237]\n",
      " [-0.62819755  1.1047147   3.41121231]\n",
      " [ 2.77052521  0.909624    2.99921379]\n",
      " [-0.7659689   0.55133917  2.89373159]\n",
      " [-0.67022344  1.06003307  2.39400584]]\n",
      "sqP = \n",
      "[[1.4399341068871307, 0.0, 0.0], [-0.05378029610966122, 0.521751219716612, 0.0], [0.04939766940143283, 0.006433581552555446, 0.5147373626947581]]\n",
      "m = \n",
      "[0.14966799 1.03999015 3.06752173]\n",
      "P = \n",
      "[[ 2.06933564e+00 -5.61274728e-02  5.18217686e-02]\n",
      " [-5.61274728e-02  6.77468273e-02  5.90222352e-05]\n",
      " [ 5.18217686e-02  5.90222352e-05  6.82310306e-02]]\n",
      "num = \n",
      "159\n",
      "SENSOR 8 4.14607416122 -1.62652743084\n",
      "\n",
      "num = \n",
      "160\n",
      "SENSOR 1 2.4585060639 -2.62149672021\n",
      "\n",
      "num = \n",
      "161\n",
      "SENSOR 2 1.39609946002 0.827146029562\n",
      "\n",
      "num = \n",
      "162\n",
      "SENSOR 3 4.01022306014 -0.221408239995\n",
      "\n",
      "num = \n",
      "163\n",
      "SENSOR 7 4.26091719102 -1.12261197694\n",
      "\n",
      "num = \n",
      "164\n",
      "ODOMETRY 3.6215575313e-05 0.0999441653561 0.000171557215833\n",
      "\n",
      "sqP = \n",
      "[[1.4385185569256123, 0.0, 0.0], [-0.039017552128629035, 0.25734113151978544, 0.0], [0.036024400460432746, 0.005691302239214995, 0.25865204857344176]]\n",
      "sigma = \n",
      "[[ 0.14966799  1.03999015  3.06752173]\n",
      " [ 3.02670511  1.03999015  3.06752173]\n",
      " [ 0.07163289  1.55467241  3.06752173]\n",
      " [ 0.22171679  1.05137275  3.58482582]\n",
      " [-2.72736912  1.03999015  3.06752173]\n",
      " [ 0.2277031   0.52530788  3.06752173]\n",
      " [ 0.07761919  1.02860754  2.55021763]]\n",
      "sigmax = \n",
      "[[ 0.14978064  1.13901619  3.08252499]\n",
      " [ 3.02681775  0.94048628  3.0789338 ]\n",
      " [ 0.07174553  1.6545667   3.07475967]\n",
      " [ 0.22182944  1.14906185  3.6069187 ]\n",
      " [-2.72725648  0.94833239  3.02714708]\n",
      " [ 0.22781574  0.62286298  3.090199  ]\n",
      " [ 0.07773183  1.12845671  2.55805344]]\n",
      "sqP = \n",
      "[[1.5073671634416048, 0.0, 0.0], [-0.008425129015116974, 0.5230416737884444, 0.0], [0.018721932541412353, 0.0029851167538389183, 0.5184044558064954]]\n",
      "m = \n",
      "[0.15445092 1.02434112 3.17480959]\n",
      "P = \n",
      "[[ 2.27183576 -0.00926821  0.02090934]\n",
      " [-0.00926821  0.06361217  0.00273985]\n",
      " [ 0.02090934  0.00273985  0.06776301]]\n",
      "num = \n",
      "165\n",
      "SENSOR 8 4.09025633382 -1.62902600145\n",
      "\n",
      "num = \n",
      "166\n",
      "SENSOR 1 2.36969383929 -2.76059077539\n",
      "\n",
      "num = \n",
      "167\n",
      "SENSOR 2 1.19862098202 1.02746191761\n",
      "\n",
      "num = \n",
      "168\n",
      "SENSOR 3 3.79856589715 -0.166995804924\n",
      "\n",
      "num = \n",
      "169\n",
      "SENSOR 7 4.28290107013 -1.22394878975\n",
      "\n",
      "num = \n",
      "170\n",
      "ODOMETRY -8.01303873469e-05 0.100119970896 0.000927184717075\n",
      "\n",
      "sqP = \n",
      "[[1.50726101293717, 0.0, 0.0], [-0.0061490384170781185, 0.25213956214623356, 0.0], [0.013872409975817221, 0.011204697822404632, 0.25970178935840915]]\n",
      "sigma = \n",
      "[[ 0.15445092  1.02434112  3.17480959]\n",
      " [ 3.16897294  1.02434112  3.17480959]\n",
      " [ 0.14215284  1.52862024  3.17480959]\n",
      " [ 0.18219574  1.04675051  3.69421317]\n",
      " [-2.86007111  1.02434112  3.17480959]\n",
      " [ 0.16674899  0.52006199  3.17480959]\n",
      " [ 0.1267061   1.00193172  2.65540601]]\n",
      "sigmax = \n",
      "[[ 0.15465869  1.123095    3.19018833]\n",
      " [-3.11400459  0.92443451  3.17206981]\n",
      " [ 0.14236061  1.62755578  3.18897272]\n",
      " [ 0.18240351  1.14503977  3.71232555]\n",
      " [-2.85986334  0.92833237  3.14703986]\n",
      " [ 0.16695677  0.61861929  3.19140162]\n",
      " [ 0.12691387  1.10107423  2.66803928]]\n",
      "sqP = \n",
      "[[1.4334635732181151, 0.0, 0.0], [0.0798953948328835, 0.5144072474558444, 0.0], [0.01483894176728429, 0.005160129616717955, 0.517809803252747]]\n",
      "m = \n",
      "[-0.63668432  1.02004925  3.27179252]\n",
      "P = \n",
      "[[2.04978499 0.08305478 0.01593398]\n",
      " [0.08305478 0.06828786 0.00427072]\n",
      " [0.01593398 0.00427072 0.06903302]]\n",
      "num = \n",
      "171\n",
      "SENSOR 8 4.07670842571 -1.65993610399\n",
      "\n",
      "num = \n",
      "172\n",
      "SENSOR 1 2.69624348373 -2.70745662616\n",
      "\n",
      "num = \n",
      "173\n",
      "SENSOR 2 1.27863825632 1.0228680341\n",
      "\n",
      "num = \n",
      "174\n",
      "SENSOR 3 3.69329884881 -0.310970841602\n",
      "\n",
      "num = \n",
      "175\n",
      "SENSOR 7 4.33458610704 -1.19229396722\n",
      "\n",
      "num = \n",
      "176\n",
      "ODOMETRY 0.000523456685193 0.100105137199 9.69165742264e-05\n",
      "\n",
      "sqP = \n",
      "[[1.4317070186807221, 0.0, 0.0], [0.05801101929702166, 0.2547990909574626, 0.0], [0.011129360037830193, 0.014227277928019293, 0.2621197019826991]]\n",
      "sigma = \n",
      "[[-0.63668432  1.02004925  3.27179252]\n",
      " [ 2.22672972  1.02004925  3.27179252]\n",
      " [-0.52066228  1.52964743  3.27179252]\n",
      " [-0.6144256   1.04850381  3.79603193]\n",
      " [-3.50009836  1.02004925  3.27179252]\n",
      " [-0.75270636  0.51045107  3.27179252]\n",
      " [-0.65894304  0.99159469  2.74755312]]\n",
      "sigmax = \n",
      "[[-0.63583727  1.10054809  3.21226148]\n",
      " [ 2.22757677  0.95899252  3.35114039]\n",
      " [-0.51981523  1.6164965   3.22198041]\n",
      " [-0.61357855  1.13030768  3.73830728]\n",
      " [ 2.783934    0.9262975   3.30692966]\n",
      " [-0.7518593   0.58351729  3.20334301]\n",
      " [-0.65809599  1.07074862  2.68624517]]\n",
      "sqP = \n",
      "[[1.4399256237218176, 0.0, 0.0], [-0.05478931377024595, 0.5183059067267446, 0.0], [0.04886641006141965, 0.008562604019524458, 0.5190477136009547]]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "m = \n",
      "[0.17131364 1.00201615 3.40346096]\n",
      "P = \n",
      "[[ 2.06926073e+00 -5.71215279e-02  5.13454396e-02]\n",
      " [-5.71215279e-02  6.45806586e-02  5.38274535e-04]\n",
      " [ 5.13454396e-02  5.38274535e-04  7.21872241e-02]]\n",
      "num = \n",
      "177\n",
      "SENSOR 8 4.19218444921 -1.62918746421\n",
      "\n",
      "num = \n",
      "178\n",
      "SENSOR 1 2.56469838469 -2.73365039673\n",
      "\n",
      "num = \n",
      "179\n",
      "SENSOR 2 1.07742306684 1.02436797734\n",
      "\n",
      "num = \n",
      "180\n",
      "SENSOR 3 3.79502409217 -0.2864736654\n",
      "\n",
      "num = \n",
      "181\n",
      "SENSOR 7 4.23474402594 -1.08843934559\n",
      "\n",
      "num = \n",
      "182\n",
      "ODOMETRY 0.000760851469688 0.0999197195975 -0.000675084333497\n",
      "\n",
      "sqP = \n",
      "[[1.438492520147925, 0.0, 0.0], [-0.03970929783326944, 0.25100563788625485, 0.0], [0.035693921827035215, 0.007791279607865863, 0.2661812615445494]]\n",
      "sigma = \n",
      "[[ 0.17131364  1.00201615  3.40346096]\n",
      " [ 3.04829868  1.00201615  3.40346096]\n",
      " [ 0.09189504  1.50402743  3.40346096]\n",
      " [ 0.24270148  1.01759871  3.93582348]\n",
      " [-2.7056714   1.00201615  3.40346096]\n",
      " [ 0.25073224  0.50000487  3.40346096]\n",
      " [ 0.0999258   0.98643359  2.87109843]]\n",
      "sigmax = \n",
      "[[ 0.17193401  1.10064697  3.4205782 ]\n",
      " [ 3.04891905  0.90234148  3.41273445]\n",
      " [ 0.09251542  1.60370536  3.41269936]\n",
      " [ 0.24332186  1.11475739  3.95993219]\n",
      " [-2.70505103  0.91129484  3.3611445 ]\n",
      " [ 0.25135261  0.59696681  3.42834914]\n",
      " [ 0.10054617  1.08603412  2.88113702]]\n",
      "sqP = \n",
      "[[1.507352518461204, 0.0, 0.0], [-0.008596360761100036, 0.5199601296069892, 0.0], [0.018797796567026737, 0.0044327399571059784, 0.5222622363977577]]\n",
      "m = \n",
      "[0.17346328 0.95665434 3.41081922]\n",
      "P = \n",
      "[[ 2.27179133 -0.00945705  0.02107258]\n",
      " [-0.00945705  0.06005816  0.00276962]\n",
      " [ 0.02107258  0.00276962  0.0724307 ]]\n",
      "num = \n",
      "183\n",
      "SENSOR 8 3.97471624618 -1.67287700529\n",
      "\n",
      "num = \n",
      "184\n",
      "SENSOR 1 2.75006267624 -2.92186387986\n",
      "\n",
      "num = \n",
      "185\n",
      "SENSOR 2 1.07790545646 1.14460483582\n",
      "\n",
      "num = \n",
      "186\n",
      "SENSOR 3 3.65459543213 -0.30509704695\n",
      "\n",
      "num = \n",
      "187\n",
      "SENSOR 7 4.07939470262 -1.19849586955\n",
      "\n",
      "num = \n",
      "188\n",
      "ODOMETRY 0.0001776465883 0.100000660883 -0.000138153337743\n",
      "\n",
      "sqP = \n",
      "[[1.5072462727887734, 0.0, 0.0], [-0.006274390778699161, 0.2449873262258676, 0.0], [0.01398084894300202, 0.011663226093353019, 0.2685129447279173]]\n",
      "sigma = \n",
      "[[ 0.17346328  0.95665434  3.41081922]\n",
      " [ 3.18795582  0.95665434  3.41081922]\n",
      " [ 0.1609145   1.446629    3.41081922]\n",
      " [ 0.20142498  0.9799808   3.94784511]\n",
      " [-2.84102927  0.95665434  3.41081922]\n",
      " [ 0.18601206  0.46667969  3.41081922]\n",
      " [ 0.14550158  0.93332789  2.87379334]]\n",
      "sigmax = \n",
      "[[ 0.17354905  1.05506141  3.42813971]\n",
      " [-3.09514372  0.85684555  3.40611235]\n",
      " [ 0.16100026  1.54524566  3.42690349]\n",
      " [ 0.20151074  1.07786515  3.9679101 ]\n",
      " [-2.8409435   0.86123658  3.38116455]\n",
      " [ 0.18609783  0.56486167  3.42937321]\n",
      " [ 0.14558735  1.03218074  2.88835578]]\n",
      "sqP = \n",
      "[[1.433465823340496, 0.0, 0.0], [0.0796308162456044, 0.5109461733364683, 0.0], [0.016534830162149673, 0.005627640212359857, 0.5222999220130145]]\n",
      "m = \n",
      "[-0.6011509   1.05184116  3.53829311]\n",
      "P = \n",
      "[[2.04974993 0.08274075 0.01771394]\n",
      " [0.08274075 0.06586635 0.00428406]\n",
      " [0.01771394 0.00428406 0.07287526]]\n",
      "num = \n",
      "189\n",
      "SENSOR 8 4.04139703382 -1.80527111094\n",
      "\n",
      "num = \n",
      "190\n",
      "SENSOR 1 2.81682858649 -2.73778261664\n",
      "\n",
      "num = \n",
      "191\n",
      "SENSOR 2 1.01157732908 1.35009000631\n",
      "\n",
      "num = \n",
      "192\n",
      "SENSOR 3 3.43641149431 -0.177181332772\n",
      "\n",
      "num = \n",
      "193\n",
      "SENSOR 7 4.1134848859 -1.19890280774\n",
      "\n",
      "num = \n",
      "194\n",
      "ODOMETRY -9.54350459669e-05 0.100059958212 0.000505361195257\n",
      "\n",
      "sqP = \n",
      "[[1.4316947756618752, 0.0, 0.0], [0.05779217015749239, 0.2500528250164784, 0.0], [0.012372708214719403, 0.014273054974732082, 0.269292505201916]]\n",
      "sigma = \n",
      "[[-0.6011509   1.05184116  3.53829311]\n",
      " [ 2.26223866  1.05184116  3.53829311]\n",
      " [-0.48556656  1.55194681  3.53829311]\n",
      " [-0.57640548  1.08038727  4.07687812]\n",
      " [-3.46454045  1.05184116  3.53829311]\n",
      " [-0.71673524  0.55173551  3.53829311]\n",
      " [-0.62589631  1.02329505  2.99970809]]\n",
      "sigmax = \n",
      "[[-0.6011114   1.13432027  3.48174818]\n",
      " [ 2.26227815  0.98806216  3.615315  ]\n",
      " [-0.48552706  1.64039675  3.49163756]\n",
      " [-0.57636599  1.16424022  4.02239128]\n",
      " [ 2.81868435  0.95700449  3.5700128 ]\n",
      " [-0.71669574  0.62714312  3.4726134 ]\n",
      " [-0.62585682  1.10434983  2.94113971]]\n",
      "sqP = \n",
      "[[1.4399281529201193, 0.0, 0.0], [-0.05656019670612582, 0.515896619001508, 0.0], [0.04673929837466741, 0.008968625428713368, 0.5227391167729019]]\n",
      "m = \n",
      "[0.20402871 1.03913851 3.66026444]\n",
      "P = \n",
      "[[ 2.06926229e+00 -5.88374683e-02  4.92004184e-02]\n",
      " [-5.88374683e-02  6.15950799e-02  1.01129179e-04]\n",
      " [ 4.92004184e-02  1.01129179e-04  7.49898469e-02]]\n",
      "num = \n",
      "195\n",
      "SENSOR 8 4.14771152129 -1.65785363504\n",
      "\n",
      "num = \n",
      "196\n",
      "SENSOR 1 2.97373538756 -2.76749218654\n",
      "\n",
      "num = \n",
      "197\n",
      "SENSOR 2 1.08572526299 1.27905533571\n",
      "\n",
      "num = \n",
      "198\n",
      "SENSOR 3 3.19721928714 -0.285485802808\n",
      "\n",
      "num = \n",
      "199\n",
      "SENSOR 7 4.30425329678 -1.23670399341\n",
      "\n",
      "num = \n",
      "200\n",
      "ODOMETRY -0.000425877997443 0.100165010148 0.000263981771762\n",
      "\n",
      "sqP = \n",
      "[[1.4384930631637487, 0.0, 0.0], [-0.040902156451729524, 0.2447898966052008, 0.0], [0.034202749858486876, 0.006128093622849628, 0.2716292790849487]]\n",
      "sigma = \n",
      "[[ 0.20402871  1.03913851  3.66026444]\n",
      " [ 3.08101484  1.03913851  3.66026444]\n",
      " [ 0.1222244   1.5287183   3.66026444]\n",
      " [ 0.27243421  1.05139469  4.203523  ]\n",
      " [-2.67295742  1.03913851  3.66026444]\n",
      " [ 0.28583302  0.54955871  3.66026444]\n",
      " [ 0.13562321  1.02688232  3.11700588]]\n",
      "sigmax = \n",
      "[[ 0.20443864  1.13712498  3.68052885]\n",
      " [ 3.08142476  0.93926266  3.66633168]\n",
      " [ 0.12263432  1.62803296  3.67245431]\n",
      " [ 0.27284414  1.14776689  4.2304376 ]\n",
      " [-2.67254749  0.94986215  3.61507897]\n",
      " [ 0.28624295  0.64556164  3.68846786]\n",
      " [ 0.13603314  1.12602474  3.13052531]]\n",
      "sqP = \n",
      "[[1.5073503598269742, 0.0, 0.0], [-0.0090703589779155, 0.516934808305723, 0.0], [0.018575920231290725, 0.004025587176241743, 0.525098715794557]]\n",
      "m = \n",
      "[0.21137374 0.98324741 3.81366327]\n",
      "P = \n",
      "[[ 2.27178636e+00 -9.98155096e-03  2.08480036e-02]\n",
      " [-9.98155096e-03  5.68254203e-02  1.84244492e-03]\n",
      " [ 2.08480036e-02  1.84244492e-03  7.46387826e-02]]\n",
      "num = \n",
      "201\n",
      "SENSOR 8 4.0209837071 -1.67421338488\n",
      "\n",
      "num = \n",
      "202\n",
      "SENSOR 1 2.98678421634 -2.79971956357\n",
      "\n",
      "num = \n",
      "203\n",
      "SENSOR 2 1.01794738917 1.50929471669\n",
      "\n",
      "num = \n",
      "204\n",
      "SENSOR 3 3.17923741534 -0.348369090478\n",
      "\n",
      "num = \n",
      "205\n",
      "SENSOR 7 4.02065742023 -1.27095421415\n",
      "\n",
      "num = \n",
      "206\n",
      "ODOMETRY 6.98085102272e-05 0.0998571294686 0.000814819876331\n",
      "\n",
      "sqP = \n",
      "[[1.5072446261387038, 0.0, 0.0], [-0.006622382847433734, 0.23828882545962368, 0.0], [0.013831864633428715, 0.00811638911010224, 0.2727298780657775]]\n",
      "sigma = \n",
      "[[ 0.21137374  0.98324741  3.81366327]\n",
      " [ 3.22586299  0.98324741  3.81366327]\n",
      " [ 0.19812897  1.45982506  3.81366327]\n",
      " [ 0.23903747  0.99948019  4.35912303]\n",
      " [-2.80311551  0.98324741  3.81366327]\n",
      " [ 0.22461851  0.50666976  3.81366327]\n",
      " [ 0.18371001  0.96701463  3.26820352]]\n",
      "sigmax = \n",
      "[[ 0.21121184  1.08119205  3.83463651]\n",
      " [-3.05748421  0.88343427  3.80527483]\n",
      " [ 0.19796708  1.55803889  3.83333745]\n",
      " [ 0.23887557  1.09680723  4.38279741]\n",
      " [-2.80327741  0.88875146  3.78044362]\n",
      " [ 0.22445661  0.60432803  3.83593189]\n",
      " [ 0.18354811  1.06550192  3.28645956]]\n",
      "sqP = \n",
      "[[1.4334660164420936, 0.0, 0.0], [0.07919799424705717, 0.507714772362252, 0.0], [0.019538512135444518, 0.0038659185451051193, 0.5244918981403256]]\n",
      "m = \n",
      "[-0.56574469  1.07098048  3.89134265]\n",
      "P = \n",
      "[[2.04961638 0.08182997 0.02069662]\n",
      " [0.08182997 0.06100947 0.00278794]\n",
      " [0.02069662 0.00278794 0.07609759]]\n",
      "num = \n",
      "207\n",
      "SENSOR 8 3.94704456339 -1.84097040455\n",
      "\n",
      "num = \n",
      "208\n",
      "SENSOR 1 3.00070004218 -2.81153261862\n",
      "\n",
      "num = \n",
      "209\n",
      "SENSOR 2 1.11963656291 1.61697540263\n",
      "\n",
      "num = \n",
      "210\n",
      "SENSOR 3 3.16238211291 -0.347631273442\n",
      "\n",
      "num = \n",
      "211\n",
      "SENSOR 7 4.11202372197 -1.30111395385\n",
      "\n",
      "num = \n",
      "212\n",
      "ODOMETRY -4.66038294942e-05 0.100189586967 -5.21580102793e-05\n",
      "\n",
      "sqP = \n",
      "[[1.431648132970367, 0.0, 0.0], [0.05715787613106918, 0.2402965717692742, 0.0], [0.014456498876586501, 0.008163392149182777, 0.2753578749860041]]\n",
      "sigma = \n",
      "[[-0.56574469  1.07098048  3.89134265]\n",
      " [ 2.29755158  1.07098048  3.89134265]\n",
      " [-0.45142894  1.55157362  3.89134265]\n",
      " [-0.53683169  1.08730726  4.4420584 ]\n",
      " [-3.42904096  1.07098048  3.89134265]\n",
      " [-0.68006044  0.59038733  3.89134265]\n",
      " [-0.59465769  1.05465369  3.3406269 ]]\n",
      "sigmax = \n",
      "[[-0.56486006  1.15528257  3.83782066]\n",
      " [ 2.2984362   1.00462538  3.96596468]\n",
      " [-0.45054431  1.64143057  3.84778608]\n",
      " [-0.53594706  1.17312139  4.39099587]\n",
      " [ 2.85502898  0.97521847  3.91964608]\n",
      " [-0.67917581  0.6680341   3.82855392]\n",
      " [-0.59377306  1.13737329  3.2846902 ]]\n",
      "sqP = \n",
      "[[1.4399267544609577, 0.0, 0.0], [-0.05852726006000825, 0.5109229398595556, 0.0], [0.044745315132950506, 0.006064199181617991, 0.5259986054001904]]\n",
      "m = \n",
      "[0.22361162 1.10885258 3.89318267]\n",
      "P = \n",
      "[[ 2.06921145e+00 -6.07641785e-02  4.72470160e-02]\n",
      " [-6.07641785e-02  5.76419759e-02 -1.62165820e-03]\n",
      " [ 4.72470160e-02 -1.62165820e-03  7.70228340e-02]]\n",
      "num = \n",
      "213\n",
      "SENSOR 8 4.01514296922 -1.96395215815\n",
      "\n",
      "num = \n",
      "214\n",
      "SENSOR 1 3.31125551661 -2.81890526713\n",
      "\n",
      "num = \n",
      "215\n",
      "SENSOR 2 1.08993866286 1.69673860324\n",
      "\n",
      "num = \n",
      "216\n",
      "SENSOR 3 3.16301966354 -0.29879355839\n",
      "\n",
      "num = \n",
      "217\n",
      "SENSOR 7 4.0697079487 -1.2813948975\n",
      "\n",
      "num = \n",
      "218\n",
      "ODOMETRY -1.1888042286e-05 0.0999378556007 0.000874157595222\n",
      "\n",
      "sqP = \n",
      "[[1.4384753899721665, 0.0, 0.0], [-0.04224207029685224, 0.2363420898165056, 0.0], [0.03284520289089531, -0.0009909738609177989, 0.275577656215327]]\n",
      "sigma = \n",
      "[[ 0.22361162  1.10885258  3.89318267]\n",
      " [ 3.1005624   1.10885258  3.89318267]\n",
      " [ 0.13912748  1.58153676  3.89318267]\n",
      " [ 0.28930202  1.10687063  4.44433798]\n",
      " [-2.65333916  1.10885258  3.89318267]\n",
      " [ 0.30809576  0.6361684   3.89318267]\n",
      " [ 0.15792121  1.11083453  3.34202735]]\n",
      "sigmax = \n",
      "[[ 0.22351285  1.20654877  3.91539543]\n",
      " [ 3.10046363  1.00874751  3.89729698]\n",
      " [ 0.13902871  1.6807589   3.90707224]\n",
      " [ 0.28920326  1.202898    4.47291592]\n",
      " [-2.65343793  1.02036761  3.84618944]\n",
      " [ 0.307997    0.73164175  3.92356017]\n",
      " [ 0.15782245  1.20977812  3.35777912]]\n",
      "sqP = \n",
      "[[1.5073402560176932, 0.0, 0.0], [-0.009459357900700594, 0.5129535539953536, 0.0], [0.018383427273298357, 0.0004826195802786639, 0.52718049870881]]\n",
      "m = \n",
      "[0.2290394  1.08411499 4.02358713]\n",
      "P = \n",
      "[[ 2.27175572e+00 -1.03913029e-02  2.06125968e-02]\n",
      " [-1.03913029e-02  5.41450426e-02  4.11703514e-04]\n",
      " [ 2.06125968e-02  4.11703514e-04  7.50005066e-02]]\n",
      "num = \n",
      "219\n",
      "SENSOR 8 4.2194758592 -1.87684781408\n",
      "\n",
      "num = \n",
      "220\n",
      "SENSOR 1 3.38916611019 -2.79022010215\n",
      "\n",
      "num = \n",
      "221\n",
      "SENSOR 2 0.953931009281 1.74716622466\n",
      "\n",
      "num = \n",
      "222\n",
      "SENSOR 3 2.97789976956 -0.385301687676\n",
      "\n",
      "num = \n",
      "223\n",
      "SENSOR 7 3.99090383462 -1.29098027348\n",
      "\n",
      "num = \n",
      "224\n",
      "ODOMETRY -0.000548398686482 0.0999074163285 -0.000586249282159\n",
      "\n",
      "sqP = \n",
      "[[1.507234459327543, 0.0, 0.0], [-0.006894284320113036, 0.23258871729770342, 0.0], [0.013675773296308005, 0.0021754631499812395, 0.27351187761943896]]\n",
      "sigma = \n",
      "[[ 0.2290394   1.08411499  4.02358713]\n",
      " [ 3.24350831  1.08411499  4.02358713]\n",
      " [ 0.21525083  1.54929242  4.02358713]\n",
      " [ 0.25639094  1.08846592  4.57061089]\n",
      " [-2.78542952  1.08411499  4.02358713]\n",
      " [ 0.24282796  0.61893756  4.02358713]\n",
      " [ 0.20168785  1.07976406  3.47656338]]\n",
      "sigmax = \n",
      "[[ 0.22990167  1.18144323  4.04627608]\n",
      " [-3.03881472  0.98469558  4.01342071]\n",
      " [ 0.2161131   1.64692425  4.04493195]\n",
      " [ 0.25725321  1.18513725  4.59595309]\n",
      " [-2.78456725  0.99044866  3.98874184]\n",
      " [ 0.24369023  0.71594371  4.0476159 ]\n",
      " [ 0.20255012  1.1776764   3.49658209]]\n",
      "sqP = \n",
      "[[1.4334662514798635, 0.0, 0.0], [0.0786226126258856, 0.505015446371726, 0.0], [0.020922014615484735, 0.000683672097328519, 0.5249016830979548]]\n",
      "m = \n",
      "[-0.56756702  1.01941153  4.17638377]\n",
      "P = \n",
      "[[2.04961339e+00 8.12728799e-02 2.19476072e-02]\n",
      " [8.12728799e-02 5.94781826e-02 1.44138856e-03]\n",
      " [2.19476072e-02 1.44138856e-03 7.52861056e-02]]\n",
      "num = \n",
      "225\n",
      "SENSOR 8 4.01544662734 -1.8776296021\n",
      "\n",
      "num = \n",
      "226\n",
      "SENSOR 1 3.58685644655 -2.88216804\n",
      "\n",
      "num = \n",
      "227\n",
      "SENSOR 2 1.09959994011 1.88380797133\n",
      "\n",
      "num = \n",
      "228\n",
      "SENSOR 3 3.10144999253 -0.263328469751\n",
      "\n",
      "num = \n",
      "229\n",
      "SENSOR 7 4.13471788936 -1.42326420591\n",
      "\n",
      "num = \n",
      "230\n",
      "ODOMETRY 0.000209771470498 0.0999383153862 0.000442898738476\n",
      "\n",
      "sqP = \n",
      "[[1.4316470905896432, 0.0, 0.0], [0.05676879478760923, 0.23718239092946478, 0.0], [0.01533031945118805, 0.002407871832477713, 0.27394395247443293]]\n",
      "sigma = \n",
      "[[-0.56756702  1.01941153  4.17638377]\n",
      " [ 2.29572716  1.01941153  4.17638377]\n",
      " [-0.45402943  1.49377631  4.17638377]\n",
      " [-0.53690638  1.02422727  4.72427167]\n",
      " [-3.4308612   1.01941153  4.17638377]\n",
      " [-0.68110461  0.54504675  4.17638377]\n",
      " [-0.59822766  1.01459578  3.62849586]]\n",
      "sigmax = \n",
      "[[-0.56870167  1.10362513  4.12262913]\n",
      " [ 2.29459251  0.95320557  4.25120531]\n",
      " [-0.45516408  1.58353777  4.13251611]\n",
      " [-0.53804103  1.11004919  4.67312394]\n",
      " [ 2.85118946  0.92367063  4.20493499]\n",
      " [-0.68223926  0.62262808  4.11343435]\n",
      " [-0.59936231  1.09712192  3.57218486]]\n",
      "sqP = \n",
      "[[1.4399205012419516, 0.0, 0.0], [-0.058704237259795185, 0.5093449702038764, 0.0], [0.045084153827372055, 0.002963409095535928, 0.525308574727864]]\n",
      "m = \n",
      "[0.22523521 1.02848262 4.20290874]\n",
      "P = \n",
      "[[ 2.06919644 -0.06110572  0.04786134]\n",
      " [-0.06110572  0.05784022 -0.00354072]\n",
      " [ 0.04786134 -0.00354072  0.07754635]]\n",
      "num = \n",
      "231\n",
      "SENSOR 8 4.11908889453 -1.84906255817\n",
      "\n",
      "num = \n",
      "232\n",
      "SENSOR 1 3.40262134444 -2.79577392772\n",
      "\n",
      "num = \n",
      "233\n",
      "SENSOR 2 0.98313589166 1.93949068611\n",
      "\n",
      "num = \n",
      "234\n",
      "SENSOR 3 2.76424553473 -0.353284737573\n",
      "\n",
      "num = \n",
      "235\n",
      "SENSOR 7 4.04514369826 -1.34914177025\n",
      "\n",
      "num = \n",
      "236\n",
      "ODOMETRY -0.000636488566826 0.0999395698257 0.000643908734646\n",
      "\n",
      "sqP = \n",
      "[[1.4384701720040607, 0.0, 0.0], [-0.042479654695129335, 0.23671859828248892, 0.0], [0.033272390641032545, -0.00898669151881108, 0.276330476806818]]\n",
      "sigma = \n",
      "[[ 0.22523521  1.02848262  4.20290874]\n",
      " [ 3.10217556  1.02848262  4.20290874]\n",
      " [ 0.1402759   1.50191982  4.20290874]\n",
      " [ 0.29177999  1.01050924  4.75556969]\n",
      " [-2.65170513  1.02848262  4.20290874]\n",
      " [ 0.31019452  0.55504543  4.20290874]\n",
      " [ 0.15869043  1.04645601  3.65024779]]\n",
      "sigmax = \n",
      "[[ 0.22588788  1.12589197  4.22524896]\n",
      " [ 3.10282823  0.92862111  4.20682605]\n",
      " [ 0.14092857  1.60087355  4.2169025 ]\n",
      " [ 0.29243266  1.10621747  4.78433777]\n",
      " [-2.65105246  0.94030834  4.1558666 ]\n",
      " [ 0.31084719  0.65020771  4.23343426]\n",
      " [ 0.1593431   1.14513529  3.66606126]]\n",
      "sqP = \n",
      "[[1.5073513222826918, 0.0, 0.0], [-0.009700886444780964, 0.5131780577539765, 0.0], [0.01844520917240956, -0.003873764702727924, 0.5275829160274531]]\n",
      "m = \n",
      "[0.22911191 1.03703961 4.28464696]\n",
      "P = \n",
      "[[ 2.27178663e+00 -1.06696225e-02  2.07204688e-02]\n",
      " [-1.06696225e-02  5.53748612e-02 -1.59994494e-03]\n",
      " [ 2.07204688e-02 -1.59994494e-03  7.56079128e-02]]\n",
      "num = \n",
      "237\n",
      "SENSOR 8 4.26368708251 -1.95171561203\n",
      "\n",
      "num = \n",
      "238\n",
      "SENSOR 1 3.59611549468 -2.8643885436\n",
      "\n",
      "num = \n",
      "239\n",
      "SENSOR 2 1.25741341306 2.05874595093\n",
      "\n",
      "num = \n",
      "240\n",
      "SENSOR 3 2.62985116185 -0.340176489157\n",
      "\n",
      "num = \n",
      "241\n",
      "SENSOR 7 4.0715354283 -1.34078750934\n",
      "\n",
      "num = \n",
      "242\n",
      "ODOMETRY -0.000749506228593 0.0998919597135 0.000431194734685\n",
      "\n",
      "sqP = \n",
      "[[1.507244715994586, 0.0, 0.0], [-0.007078891977152982, 0.23521213938556182, 0.0], [0.01374724923312799, -0.006388401766633321, 0.27455074980969235]]\n",
      "sigma = \n",
      "[[ 0.22911191  1.03703961  4.28464696]\n",
      " [ 3.24360134  1.03703961  4.28464696]\n",
      " [ 0.21495413  1.50746389  4.28464696]\n",
      " [ 0.25660641  1.02426281  4.83374846]\n",
      " [-2.78537752  1.03703961  4.28464696]\n",
      " [ 0.2432697   0.56661533  4.28464696]\n",
      " [ 0.20161741  1.04981641  3.73554546]]\n",
      "sigmax = \n",
      "[[ 0.22911933  1.13438203  4.30728256]\n",
      " [-3.03957654  0.93761311  4.27453321]\n",
      " [ 0.21496155  1.60511702  4.30590218]\n",
      " [ 0.25661383  1.12094616  4.85905155]\n",
      " [-2.7853701   0.94335174  4.24985471]\n",
      " [ 0.24327712  0.66362754  4.3086584 ]\n",
      " [ 0.20162483  1.14774432  3.75549646]]\n",
      "sqP = \n",
      "[[1.433467380170428, 0.0, 0.0], [0.07850897736831594, 0.5062921639080511, 0.0], [0.02090228963469682, -0.003992544349736259, 0.5254345157841362]]\n",
      "m = \n",
      "[-0.55529331  1.05168071  4.44960168]\n",
      "P = \n",
      "[[ 2.04966909e+00  8.14385048e-02  2.17392788e-02]\n",
      " [ 8.14385048e-02  6.18736509e-02 -1.80891284e-04]\n",
      " [ 2.17392788e-02 -1.80891284e-04  7.50069891e-02]]\n",
      "num = \n",
      "243\n",
      "SENSOR 8 4.21795338302 -1.92674399779\n",
      "\n",
      "num = \n",
      "244\n",
      "SENSOR 1 3.64715670833 -2.88849823757\n",
      "\n",
      "num = \n",
      "245\n",
      "SENSOR 2 1.15505103131 2.09289492551\n",
      "\n",
      "num = \n",
      "246\n",
      "SENSOR 3 2.52001295552 -0.472631064422\n",
      "\n",
      "num = \n",
      "247\n",
      "SENSOR 7 3.95528847465 -1.41585544885\n",
      "\n",
      "num = \n",
      "248\n",
      "ODOMETRY 0.000406907201319 0.0998586394002 4.54817335274e-05\n",
      "\n",
      "sqP = \n",
      "[[1.4316665437132792, 0.0, 0.0], [0.056883710219455916, 0.242152626301736, 0.0], [0.015184596533996042, -0.004314004308868719, 0.2734187384563242]]\n",
      "sigma = \n",
      "[[-0.55529331  1.05168071  4.44960168]\n",
      " [ 2.30803978  1.05168071  4.44960168]\n",
      " [-0.44152589  1.53598596  4.44960168]\n",
      " [-0.52492411  1.0430527   4.99643916]\n",
      " [-3.41862639  1.05168071  4.44960168]\n",
      " [-0.66906073  0.56737546  4.44960168]\n",
      " [-0.5856625   1.06030872  3.9027642 ]]\n",
      "sigmax = \n",
      "[[-0.55561162  1.13652399  4.39687578]\n",
      " [ 2.30772147  0.98458381  4.52360445]\n",
      " [-0.4418442   1.62626633  4.40684822]\n",
      " [-0.52524242  1.12945786  4.94631379]\n",
      " [ 2.8642406   0.95561804  4.47699451]\n",
      " [-0.66937904  0.64568471  4.38758503]\n",
      " [-0.58598081  1.14351188  3.84748639]]\n",
      "sqP = \n",
      "[[1.4399216125209122, 0.0, 0.0], [-0.05905319774471529, 0.5117174247216612, 0.0], [0.04422041799869682, -0.0006030277456838527, 0.5250408596441954]]\n",
      "m = \n",
      "[0.24095755 1.07572897 4.52953548]\n",
      "P = \n",
      "[[ 2.06926062 -0.06160706  0.04690205]\n",
      " [-0.06160706  0.06091363 -0.00490019]\n",
      " [ 0.04690205 -0.00490019  0.07545509]]\n",
      "num = \n",
      "249\n",
      "SENSOR 8 4.36749740434 -2.0474823273\n",
      "\n",
      "num = \n",
      "250\n",
      "SENSOR 1 3.68174071666 -2.9201773312\n",
      "\n",
      "num = \n",
      "251\n",
      "SENSOR 2 1.22696609171 2.26296774635\n",
      "\n",
      "num = \n",
      "252\n",
      "SENSOR 3 2.41868212361 -0.336817224032\n",
      "\n",
      "num = \n",
      "253\n",
      "SENSOR 7 3.98911047334 -1.47028485405\n",
      "\n",
      "num = \n",
      "254\n",
      "ODOMETRY 0.00020261199028 0.0999232742711 -0.00079340859659\n",
      "\n",
      "sqP = \n",
      "[[1.4384924803379298, 0.0, 0.0], [-0.042827515192546406, 0.24306260850317873, 0.0], [0.03260499937631855, -0.014415190959167438, 0.27236778482534363]]\n",
      "sigma = \n",
      "[[ 0.24095755  1.07572897  4.52953548]\n",
      " [ 3.11794251  1.07572897  4.52953548]\n",
      " [ 0.15530252  1.56185418  4.52953548]\n",
      " [ 0.30616755  1.04689858  5.07427105]\n",
      " [-2.63602741  1.07572897  4.52953548]\n",
      " [ 0.32661258  0.58960375  4.52953548]\n",
      " [ 0.17574755  1.10455935  3.98479991]]\n",
      "sigmax = \n",
      "[[ 0.24140994  1.17269298  4.55340446]\n",
      " [ 3.1183949   0.9758973   4.53185631]\n",
      " [ 0.15575491  1.66050471  4.54502165]\n",
      " [ 0.30661994  1.14210111  5.10440784]\n",
      " [-2.63557502  0.98838224  4.48113819]\n",
      " [ 0.32706497  0.68417028  4.56161226]\n",
      " [ 0.17619994  1.20287266  4.00229962]]\n",
      "sqP = \n",
      "[[1.507367870836667, 0.0, 0.0], [-0.010242179626095952, 0.5162434901789068, 0.0], [0.01817777974662536, -0.006545696489906422, 0.5254822537223133]]\n",
      "m = \n",
      "[0.24585409 1.05037001 4.62797768]\n",
      "P = \n",
      "[[ 2.27183468 -0.01129737  0.02038219]\n",
      " [-0.01129737  0.05898189 -0.00292442]\n",
      " [ 0.02038219 -0.00292442  0.07252737]]\n",
      "num = \n",
      "255\n",
      "SENSOR 8 4.3737723756 -1.93110738924\n",
      "\n",
      "num = \n",
      "256\n",
      "SENSOR 1 3.8479658409 -2.96461382002\n",
      "\n",
      "num = \n",
      "257\n",
      "SENSOR 2 1.44650342879 2.19184285471\n",
      "\n",
      "num = \n",
      "258\n",
      "SENSOR 3 2.4378797797 -0.376196945898\n",
      "\n",
      "num = \n",
      "259\n",
      "SENSOR 7 3.99144004155 -1.53192738274\n",
      "\n",
      "num = \n",
      "260\n",
      "ODOMETRY 0.000476456640183 0.0999946391606 0.000673997332114\n",
      "\n",
      "sqP = \n",
      "[[1.507260655551199, 0.0, 0.0], [-0.007495299431141028, 0.24274618748819418, 0.0], [0.01352267096543781, -0.011629701058851914, 0.2687178036632163]]\n",
      "sigma = \n",
      "[[ 0.24585409  1.05037001  4.62797768]\n",
      " [ 3.2603754   1.05037001  4.62797768]\n",
      " [ 0.23086349  1.53586239  4.62797768]\n",
      " [ 0.27289943  1.02711061  5.16541329]\n",
      " [-2.76866723  1.05037001  4.62797768]\n",
      " [ 0.26084468  0.56487764  4.62797768]\n",
      " [ 0.21880874  1.07362942  4.09054207]]\n",
      "sigmax = \n",
      "[[ 0.24526329  1.14728365  4.65231712]\n",
      " [-3.02340071  0.95115324  4.61611631]\n",
      " [ 0.23027269  1.63312998  4.65086165]\n",
      " [ 0.27230863  1.12333061  5.19236457]\n",
      " [-2.76925802  0.9573223   4.59155265]\n",
      " [ 0.26025389  0.66141553  4.65376713]\n",
      " [ 0.21821795  1.1711658   4.11225187]]\n",
      "sqP = \n",
      "[[1.4334666542575183, 0.0, 0.0], [0.07800653282008742, 0.5099618352372339, 0.0], [0.022200119336800425, -0.006652048241494688, 0.522357176221602]]\n",
      "m = \n",
      "[-0.53976018  1.07553227  4.72942325]\n",
      "P = \n",
      "[[ 2.04974435e+00  8.11815628e-02  2.29201754e-02]\n",
      " [ 8.11815628e-02  6.57270096e-02 -1.23327267e-03]\n",
      " [ 2.29201754e-02 -1.23327267e-03  7.16225495e-02]]\n",
      "num = \n",
      "261\n",
      "SENSOR 8 4.18501121114 -1.96001604794\n",
      "\n",
      "num = \n",
      "262\n",
      "SENSOR 1 3.94177735119 -2.84551297654\n",
      "\n",
      "num = \n",
      "263\n",
      "SENSOR 2 1.18038634345 2.41547551073\n",
      "\n",
      "num = \n",
      "264\n",
      "SENSOR 3 2.25386927139 -0.458608006904\n",
      "\n",
      "num = \n",
      "265\n",
      "SENSOR 7 4.01204561994 -1.48975256344\n",
      "\n",
      "num = \n",
      "266\n",
      "ODOMETRY 0.000136092271998 0.0999996467097 -0.000522848477433\n",
      "\n",
      "sqP = \n",
      "[[1.4316928252968, 0.0, 0.0], [0.0567031987395262, 0.2500235126126622, 0.0], [0.016009143130082205, -0.008563363797458546, 0.26700735132500336]]\n",
      "sigma = \n",
      "[[-0.53976018  1.07553227  4.72942325]\n",
      " [ 2.32362547  1.07553227  4.72942325]\n",
      " [-0.42635378  1.57557929  4.72942325]\n",
      " [-0.50774189  1.05840554  5.26343795]\n",
      " [-3.40314583  1.07553227  4.72942325]\n",
      " [-0.65316658  0.57548524  4.72942325]\n",
      " [-0.57177847  1.092659    4.19540855]]\n",
      "sigmax = \n",
      "[[-0.53860972  1.16133534  4.67807385]\n",
      " [ 2.32477593  1.00713057  4.80236251]\n",
      " [-0.42520333  1.66664208  4.68811348]\n",
      " [-0.50659144  1.14580848  5.21486167]\n",
      " [ 2.88118993  0.97892619  4.75523396]\n",
      " [-0.65201612  0.65492627  4.66869393]\n",
      " [-0.57062801  1.17677425  4.14133867]]\n",
      "sqP = \n",
      "[[1.439920884890673, 0.0, 0.0], [-0.05957758936102281, 0.5155890831640124, 0.0], [0.04319301240331869, -0.002646400721939733, 0.5217430150453866]]\n",
      "m = \n",
      "[0.25771179 1.07450988 4.77353538]\n",
      "P = \n",
      "[[ 2.06929672 -0.06220602  0.04576389]\n",
      " [-0.06220602  0.06419783 -0.00550097]\n",
      " [ 0.04576389 -0.00550097  0.07200471]]\n",
      "num = \n",
      "267\n",
      "SENSOR 8 4.40946468491 -2.02094252774\n",
      "\n",
      "num = \n",
      "268\n",
      "SENSOR 1 3.91922179189 -2.87072202395\n",
      "\n",
      "num = \n",
      "269\n",
      "SENSOR 2 1.44580310453 2.32508479098\n",
      "\n",
      "num = \n",
      "270\n",
      "SENSOR 3 2.15756119315 -0.424067573571\n",
      "\n",
      "num = \n",
      "271\n",
      "SENSOR 7 4.06104474419 -1.57224455589\n",
      "\n",
      "num = \n",
      "272\n",
      "ODOMETRY 0.000302269878199 0.0999353432319 0.000539895064198\n",
      "\n",
      "sqP = \n",
      "[[1.4385050284656202, 0.0, 0.0], [-0.043243518782752004, 0.24965542234514831, 0.0], [0.03181350953660717, -0.01652374649414967, 0.2659315273663102]]\n",
      "sigma = \n",
      "[[ 0.25771179  1.07450988  4.77353538]\n",
      " [ 3.13472185  1.07450988  4.77353538]\n",
      " [ 0.17122476  1.57382072  4.77353538]\n",
      " [ 0.32133881  1.04146239  5.30539843]\n",
      " [-2.61929826  1.07450988  4.77353538]\n",
      " [ 0.34419883  0.57519903  4.77353538]\n",
      " [ 0.19408478  1.10755737  4.24167233]]\n",
      "sigmax = \n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 0.25732504  1.17120364  4.79903531]\n",
      " [ 3.1343351   0.9745125   4.77420884]\n",
      " [ 0.170838    1.67235574  4.79058766]\n",
      " [ 0.32095206  1.13633909  5.33699495]\n",
      " [-2.61968502  0.98784929  4.72363677]\n",
      " [ 0.34381208  0.66932872  4.80729233]\n",
      " [ 0.19369802  1.20567686  4.26097247]]\n",
      "sqP = \n",
      "[[1.5073748101340372, 0.0, 0.0], [-0.010741411713443855, 0.5194538797867834, 0.0], [0.017862612061115366, -0.007274102854644, 0.5221487025263761]]\n",
      "m = \n",
      "[0.26180843 1.06561659 4.88331819]\n",
      "P = \n",
      "[[ 2.27185473 -0.01186431  0.02000942]\n",
      " [-0.01186431  0.06185757 -0.00326053]\n",
      " [ 0.02000942 -0.00326053  0.06955618]]\n",
      "num = \n",
      "273\n",
      "SENSOR 8 4.5165550898 -2.00275541219\n",
      "\n",
      "num = \n",
      "274\n",
      "SENSOR 1 4.25083690758 -2.88476512783\n",
      "\n",
      "num = \n",
      "275\n",
      "SENSOR 2 1.59664742666 2.36949118143\n",
      "\n",
      "num = \n",
      "276\n",
      "SENSOR 3 2.18656915806 -0.37303565508\n",
      "\n",
      "num = \n",
      "277\n",
      "SENSOR 7 4.05007373159 -1.58049943139\n",
      "\n",
      "num = \n",
      "278\n",
      "ODOMETRY 0.00013361527452 0.100074513275 0.000560546767954\n",
      "\n",
      "sqP = \n",
      "[[1.5072673061300008, 0.0, 0.0], [-0.00787140179645762, 0.24858722509802134, 0.0], [0.013275296615969899, -0.012695889728843926, 0.26309459094970294]]\n",
      "sigma = \n",
      "[[ 0.26180843  1.06561659  4.88331819]\n",
      " [ 3.27634304  1.06561659  4.88331819]\n",
      " [ 0.24606562  1.56279104  4.88331819]\n",
      " [ 0.28835902  1.04022481  5.40950737]\n",
      " [-2.75272619  1.06561659  4.88331819]\n",
      " [ 0.27755123  0.56844214  4.88331819]\n",
      " [ 0.23525783  1.09100837  4.35712901]]\n",
      "sigmax = \n",
      "[[ 0.26265059  1.16213866  4.90921341]\n",
      " [-3.0060001   0.96659123  4.86986264]\n",
      " [ 0.24690779  1.65970879  4.90769073]\n",
      " [ 0.28920118  1.13602541  5.43795588]\n",
      " [-2.75188402  0.97315393  4.84540078]\n",
      " [ 0.27839339  0.6645446   4.91072966]\n",
      " [ 0.2361      1.18818387  4.38045268]]\n",
      "sqP = \n",
      "[[1.4334660631271947, 0.0, 0.0], [0.07758665153309957, 0.5128158409345315, 0.0], [0.023376098004779224, -0.007021745237442772, 0.5194570136542898]]\n",
      "m = \n",
      "[-0.52696153  1.03778518  5.05203795]\n",
      "P = \n",
      "[[ 2.04979620e+00  8.09078071e-02  2.40927128e-02]\n",
      " [ 8.09078071e-02  6.84731661e-02 -1.17675820e-03]\n",
      " [ 2.40927128e-02 -1.17675820e-03  6.83216076e-02]]\n",
      "num = \n",
      "279\n",
      "SENSOR 8 4.4359853347 -1.98886105814\n",
      "\n",
      "num = \n",
      "280\n",
      "SENSOR 1 4.34929216262 -2.84640872927\n",
      "\n",
      "num = \n",
      "281\n",
      "SENSOR 2 1.51357200054 2.46096763847\n",
      "\n",
      "num = \n",
      "282\n",
      "SENSOR 3 2.1177123351 -0.433574921219\n",
      "\n",
      "num = \n",
      "283\n",
      "SENSOR 7 3.63185364294 -1.65552297298\n",
      "\n",
      "num = \n",
      "284\n",
      "ODOMETRY -0.000748347401324 0.0999214044639 0.000183054107003\n",
      "\n",
      "sqP = \n",
      "[[1.4317109329835584, 0.0, 0.0], [0.056511272784602246, 0.2554988105560179, 0.0], [0.01682791706028511, -0.008327730385058135, 0.2607087987787715]]\n",
      "sigma = \n",
      "[[-0.52696153  1.03778518  5.05203795]\n",
      " [ 2.33646034  1.03778518  5.05203795]\n",
      " [-0.41393898  1.5487828   5.05203795]\n",
      " [-0.49330569  1.02112972  5.57345554]\n",
      " [-3.39038339  1.03778518  5.05203795]\n",
      " [-0.63998407  0.52678756  5.05203795]\n",
      " [-0.56061736  1.05444064  4.53062035]]\n",
      "sigmax = \n",
      "[[-0.52626736  1.12429022  5.00172109]\n",
      " [ 2.3371545   0.96842231  5.12417463]\n",
      " [-0.41324482  1.64041075  5.01179834]\n",
      " [-0.49261153  1.10927891  5.52607803]\n",
      " [ 2.89349608  0.94078858  5.07666655]\n",
      " [-0.63928991  0.60706583  4.99228591]\n",
      " [-0.5599232   1.13920356  4.47742114]]\n",
      "sqP = \n",
      "[[1.4399209265512583, 0.0, 0.0], [-0.05994943005820284, 0.5182985137237667, 0.0], [0.04243677374298857, -0.002330354862242512, 0.5185661806682447]]\n",
      "m = \n",
      "[0.25557537 1.16665573 5.10357698]\n",
      "P = \n",
      "[[ 2.06932521 -0.062798    0.04479748]\n",
      " [-0.062798    0.06780991 -0.00522348]\n",
      " [ 0.04479748 -0.00522348  0.06806921]]\n",
      "num = \n",
      "285\n",
      "SENSOR 8 4.33976493299 -2.0786008695\n",
      "\n",
      "num = \n",
      "286\n",
      "SENSOR 1 4.26489841524 -2.98062522043\n",
      "\n",
      "num = \n",
      "287\n",
      "SENSOR 2 1.61966127202 2.46647468873\n",
      "\n",
      "num = \n",
      "288\n",
      "SENSOR 3 2.02261768412 -0.484039818089\n",
      "\n",
      "num = \n",
      "289\n",
      "SENSOR 7 4.09245647518 -1.63859759267\n",
      "\n",
      "num = \n",
      "290\n",
      "ODOMETRY 0.000428845906387 0.10008081915 0.000352531758719\n",
      "\n",
      "sqP = \n",
      "[[1.4385149304649671, 0.0, 0.0], [-0.04365474017370015, 0.2567180792508367, 0.0], [0.031141478824711653, -0.01505154079898907, 0.25859788782555665]]\n",
      "sigma = \n",
      "[[ 0.25557537  1.16665573  5.10357698]\n",
      " [ 3.13260523  1.16665573  5.10357698]\n",
      " [ 0.16826589  1.68009189  5.10357698]\n",
      " [ 0.31785833  1.13655265  5.62077276]\n",
      " [-2.62145449  1.16665573  5.10357698]\n",
      " [ 0.34288485  0.65321957  5.10357698]\n",
      " [ 0.19329241  1.19675881  4.58638121]]\n",
      "sigmax = \n",
      "[[ 0.25501008  1.26335036  5.12876497]\n",
      " [ 3.13203994  1.06673906  5.10454978]\n",
      " [ 0.1677006   1.77861456  5.12023739]\n",
      " [ 0.31729304  1.23149203  5.65193044]\n",
      " [-2.62201978  1.07991174  5.05398094]\n",
      " [ 0.34231956  0.74734953  5.13710067]\n",
      " [ 0.19272712  1.29483373  4.60550182]]\n",
      "sqP = \n",
      "[[1.5073820793725965, 0.0, 0.0], [-0.010936387222843746, 0.522846606646561, 0.0], [0.01759134679283026, -0.006347161117359418, 0.5183866132207525]]\n",
      "m = \n",
      "[0.25765551 1.1151583  5.13245577]\n",
      "P = \n",
      "[[ 2.27187685 -0.01210036  0.01962355]\n",
      " [-0.01210036  0.06534657 -0.00258309]\n",
      " [ 0.01962355 -0.00258309  0.06497142]]\n",
      "num = \n",
      "291\n",
      "SENSOR 8 4.66591979904 -2.11042811091\n",
      "\n",
      "num = \n",
      "292\n",
      "SENSOR 1 4.5400120979 -2.94675157545\n",
      "\n",
      "num = \n",
      "293\n",
      "SENSOR 2 1.66151980155 2.4772319982\n",
      "\n",
      "num = \n",
      "294\n",
      "SENSOR 3 1.69406105599 -0.528339655367\n",
      "\n",
      "num = \n",
      "295\n",
      "SENSOR 7 3.93440624897 -1.69607534156\n",
      "\n",
      "num = \n",
      "296\n",
      "ODOMETRY 0.000241280703103 0.100190717382 0.00021908908565\n",
      "\n",
      "sqP = \n",
      "[[1.507274644781958, 0.0, 0.0], [-0.008027975551361073, 0.2555036611724698, 0.0], [0.013019226499727327, -0.009700712896824698, 0.2543773105350296]]\n",
      "sigma = \n",
      "[[ 0.25765551  1.1151583   5.13245577]\n",
      " [ 3.2722048   1.1151583   5.13245577]\n",
      " [ 0.24159956  1.62616563  5.13245577]\n",
      " [ 0.28369396  1.09575688  5.64121039]\n",
      " [-2.75689378  1.1151583   5.13245577]\n",
      " [ 0.27371146  0.60415098  5.13245577]\n",
      " [ 0.23161706  1.13455973  4.62370115]]\n",
      "sigmax = \n",
      "[[ 0.25843689  1.21192451  5.15799928]\n",
      " [-3.01019913  1.01593554  5.11937858]\n",
      " [ 0.24238093  1.72332947  5.15644238]\n",
      " [ 0.28447534  1.19182525  5.6692646 ]\n",
      " [-2.7561124   1.02240836  5.09485766]\n",
      " [ 0.27449284  0.70049461  5.15954959]\n",
      " [ 0.23239843  1.23195818  4.64671664]]\n",
      "sqP = \n",
      "[[1.43346425764304, 0.0, 0.0], [0.07774947056457912, 0.5161496405325224, 0.0], [0.022961269971781228, -0.005269854879441646, 0.5150910633457141]]\n",
      "m = \n",
      "[-0.52788863  1.08645689  5.37754494]\n",
      "P = \n",
      "[[ 2.04982910e+00  8.11408904e-02  2.36998659e-02]\n",
      " [ 8.11408904e-02  7.11269942e-02 -5.74265642e-04]\n",
      " [ 2.36998659e-02 -5.74265642e-04  6.48989522e-02]]\n",
      "num = \n",
      "297\n",
      "SENSOR 8 4.65394245626 -2.10046665109\n",
      "\n",
      "num = \n",
      "298\n",
      "SENSOR 1 4.6922590945 -2.8776113437\n",
      "\n",
      "num = \n",
      "299\n",
      "SENSOR 2 1.84720641205 2.51332375216\n",
      "\n",
      "num = \n",
      "300\n",
      "SENSOR 3 1.7878592279 -0.6464169453\n",
      "\n",
      "num = \n",
      "301\n",
      "SENSOR 7 4.08108369627 -1.67639330679\n",
      "\n",
      "num = \n",
      "302\n",
      "ODOMETRY -0.00119608488614 0.0998300419662 -0.000785692437084\n",
      "\n",
      "sqP = \n",
      "[[1.4317224233255141, 0.0, 0.0], [0.056673618505002694, 0.2606052477783681, 0.0], [0.016553394335097633, -0.005803437999990461, 0.2541481013035293]]\n",
      "sigma = \n",
      "[[-0.52788863  1.08645689  5.37754494]\n",
      " [ 2.33555622  1.08645689  5.37754494]\n",
      " [-0.41454139  1.60766738  5.37754494]\n",
      " [-0.49478184  1.07485001  5.88584115]\n",
      " [-3.39133347  1.08645689  5.37754494]\n",
      " [-0.64123586  0.56524639  5.37754494]\n",
      " [-0.56099542  1.09806376  4.86924874]]\n",
      "sigmax = \n",
      "[[-0.52742826  1.17302107  5.32709871]\n",
      " [ 2.33601659  1.01707102  5.44982068]\n",
      " [-0.41408102  1.69938179  5.33721324]\n",
      " [-0.49432147  1.16303656  5.8382879 ]\n",
      " [ 2.8923122   0.98936846  5.40228394]\n",
      " [-0.64077549  0.64554939  5.31763161]\n",
      " [-0.56053505  1.1829107   4.81596481]]\n",
      "sqP = \n",
      "[[1.4399230152091218, 0.0, 0.0], [-0.059737327880059865, 0.5208719834248722, 0.0], [0.042415379758493195, -0.0009735103745096017, 0.515284731864072]]\n",
      "m = \n",
      "[0.2774688  1.05359278 5.48210866]\n",
      "P = \n",
      "[[ 2.06934588 -0.06270233  0.04452759]\n",
      " [-0.06270233  0.07090918 -0.00398575]\n",
      " [ 0.04452759 -0.00398575  0.06444243]]\n",
      "num = \n",
      "303\n",
      "SENSOR 8 4.77404605226 -2.08170107067\n",
      "\n",
      "num = \n",
      "304\n",
      "SENSOR 1 4.33976416105 -2.90292850512\n",
      "\n",
      "num = \n",
      "305\n",
      "SENSOR 2 1.88969783614 2.58825155721\n",
      "\n",
      "num = \n",
      "306\n",
      "SENSOR 3 1.73764363829 -0.49745195748\n",
      "\n",
      "num = \n",
      "307\n",
      "SENSOR 7 3.96406958388 -1.66060095497\n",
      "\n",
      "num = \n",
      "308\n",
      "ODOMETRY -0.000283673180471 0.0998692948249 -6.45801742076e-05\n",
      "\n",
      "sqP = \n",
      "[[1.4385221176775667, 0.0, 0.0], [-0.043588021476442605, 0.2626961517627275, 0.0], [0.03095370259289791, -0.01003647214509194, 0.25176093238061725]]\n",
      "sigma = \n",
      "[[ 0.2774688   1.05359278  5.48210866]\n",
      " [ 3.15451303  1.05359278  5.48210866]\n",
      " [ 0.19029275  1.57898508  5.48210866]\n",
      " [ 0.3393762   1.03351983  5.98563053]\n",
      " [-2.59957544  1.05359278  5.48210866]\n",
      " [ 0.36464484  0.52820047  5.48210866]\n",
      " [ 0.21556139  1.07366572  4.9785868 ]]\n",
      "sigmax = \n",
      "[[ 0.27548702  1.14963715  5.50933946]\n",
      " [-3.13065405  0.9537696   5.48093825]\n",
      " [ 0.18831098  1.6770356   5.50087389]\n",
      " [ 0.33739442  1.1276955   6.01875123]\n",
      " [-2.60155722  0.96800986  5.43071215]\n",
      " [ 0.36266306  0.62150925  5.51759822]\n",
      " [ 0.21357961  1.17121081  4.99982337]]\n",
      "sqP = \n",
      "[[1.439064049624724, 0.0, 0.0], [0.06928514189460168, 0.5211822695382682, 0.0], [0.026335306861585632, -0.007394016699836737, 0.514636978727702]]\n",
      "m = \n",
      "[-0.51377401  1.09299786  5.44647989]\n",
      "P = \n",
      "[[2.06678400e+00 7.28486583e-02 2.75180930e-02]\n",
      " [7.28486583e-02 7.32292647e-02 9.14892553e-04]\n",
      " [2.75180930e-02 9.14892553e-04 6.26805940e-02]]\n",
      "num = \n",
      "309\n",
      "SENSOR 8 4.72928958355 -2.11808295378\n",
      "\n",
      "num = \n",
      "310\n",
      "SENSOR 1 4.85446874563 -2.96828803681\n",
      "\n",
      "num = \n",
      "311\n",
      "SENSOR 2 1.94629939838 2.61814643875\n",
      "\n",
      "num = \n",
      "312\n",
      "SENSOR 3 1.60770645745 -0.563753684955\n",
      "\n",
      "num = \n",
      "313\n",
      "SENSOR 7 3.89182668135 -1.78660920606\n",
      "\n",
      "num = \n",
      "314\n",
      "ODOMETRY -0.000182066224786 0.0998742815469 -0.000565724685502\n",
      "\n",
      "sqP = \n",
      "[[1.4376313840860329, 0.0, 0.0], [0.05067269613921606, 0.2658223890508074, 0.0], [0.01914127176115794, -0.000207082988719575, 0.24962804891115875]]\n",
      "sigma = \n",
      "[[-0.51377401  1.09299786  5.44647989]\n",
      " [ 2.36148875  1.09299786  5.44647989]\n",
      " [-0.41242862  1.62464264  5.44647989]\n",
      " [-0.47549147  1.0925837   5.94573599]\n",
      " [-3.38903678  1.09299786  5.44647989]\n",
      " [-0.61511941  0.56135308  5.44647989]\n",
      " [-0.55205656  1.09341203  4.94722379]]\n",
      "sigmax = \n",
      "[[-0.51412227  1.1799597   5.39737271]\n",
      " [ 2.3611405   1.02202666  5.51674342]\n",
      " [-0.41277687  1.71612654  5.40642278]\n",
      " [-0.47583972  1.1813613   5.89999309]\n",
      " [ 2.89380027  0.99617737  5.47096802]\n",
      " [-0.61546766  0.64290044  5.38882658]\n",
      " [-0.55240481  1.17843066  4.89482428]]\n",
      "sqP = \n",
      "[[1.4391726225820034, 0.0, 0.0], [-0.06100315843968696, 0.5232322750642268, 0.0], [0.04188913167405069, 0.0017371782283295883, 0.5131104536977097]]\n",
      "m = \n",
      "[0.28847507 1.12996992 5.62106977]\n",
      "P = \n",
      "[[ 2.06710448 -0.06397642  0.04399357]\n",
      " [-0.06397642  0.07184792 -0.0035768 ]\n",
      " [ 0.04399357 -0.0035768   0.06369047]]\n",
      "num = \n",
      "315\n",
      "SENSOR 8 4.66229866868 -2.20515623143\n",
      "\n",
      "num = \n",
      "316\n",
      "SENSOR 1 4.80809797253 -3.00294348241\n",
      "\n",
      "num = \n",
      "317\n",
      "SENSOR 2 2.13430608854 2.71645106462\n",
      "\n",
      "num = \n",
      "318\n",
      "SENSOR 3 1.376952877 -0.663110570535\n",
      "\n",
      "num = \n",
      "319\n",
      "SENSOR 7 3.8811618647 -1.8038777696\n",
      "\n",
      "num = \n",
      "320\n",
      "ODOMETRY 0.000446460674214 0.0998339319617 0.000172792242606\n",
      "\n",
      "sqP = \n",
      "[[1.4377428412567284, 0.0, 0.0], [-0.04449781590135301, 0.26432530650286734, 0.0], [0.03059905206367859, -0.00838060140148693, 0.25036759897620703]]\n",
      "sigma = \n",
      "[[ 0.28847507  1.12996992  5.62106977]\n",
      " [ 3.16396076  1.12996992  5.62106977]\n",
      " [ 0.19947944  1.65862053  5.62106977]\n",
      " [ 0.34967318  1.11320871  6.12180496]\n",
      " [-2.58701061  1.12996992  5.62106977]\n",
      " [ 0.37747071  0.6013193   5.62106977]\n",
      " [ 0.22727697  1.14673112  5.12033457]]\n",
      "sigmax = \n",
      "[[ 0.28772728  1.22572245  5.64946563]\n",
      " [-3.11997234  1.03012021  5.61885413]\n",
      " [ 0.19873165  1.75651789  5.64084294]\n",
      " [ 0.34892539  1.2070453   6.15600389]\n",
      " [-2.5877584   1.04505516  5.56849262]\n",
      " [ 0.37672291  0.69416912  5.65786357]\n",
      " [ 0.22652918  1.24404109  5.14282106]]\n",
      "sqP = \n",
      "[[1.439156146088845, 0.0, 0.0], [0.0688644165617777, 0.5220317372336042, 0.0], [0.02716634152624791, -0.006563653799302037, 0.5139423514773834]]\n",
      "m = \n",
      "[-0.48374846  1.24045884  5.74099823]\n",
      "P = \n",
      "[[2.06708799e+00 7.25648287e-02 2.84422625e-02]\n",
      " [7.25648287e-02 7.47020696e-02 1.72191002e-03]\n",
      " [2.84422625e-02 1.72191002e-03 6.16541033e-02]]\n",
      "num = \n",
      "321\n",
      "SENSOR 8 5.02546804585 -2.19510092046\n",
      "\n",
      "num = \n",
      "322\n",
      "SENSOR 1 5.01349268035 -2.96140277102\n",
      "\n",
      "num = \n",
      "323\n",
      "SENSOR 2 2.16725029068 2.71920290634\n",
      "\n",
      "num = \n",
      "324\n",
      "SENSOR 3 1.6217305118 -0.701135122275\n",
      "\n",
      "num = \n",
      "325\n",
      "SENSOR 7 3.94079630166 -1.73582055105\n",
      "\n",
      "num = \n",
      "326\n",
      "ODOMETRY -0.000192785040713 0.099962525761 -0.000198322747385\n",
      "\n",
      "sqP = \n",
      "[[1.437737108571315, 0.0, 0.0], [0.050471555794812974, 0.26861625360584523, 0.0], [0.01978265868903716, 0.002693241549376655, 0.24749847706513275]]\n",
      "sigma = \n",
      "[[-0.48374846  1.24045884  5.74099823]\n",
      " [ 2.39172575  1.24045884  5.74099823]\n",
      " [-0.38280535  1.77769134  5.74099823]\n",
      " [-0.44418315  1.24584532  6.23599519]\n",
      " [-3.35922268  1.24045884  5.74099823]\n",
      " [-0.58469157  0.70322633  5.74099823]\n",
      " [-0.52331378  1.23507235  5.24600128]]\n",
      "sigmax = \n",
      "[[-0.48312921  1.32885832  5.69460485]\n",
      " [ 2.39234501  1.16737203  5.80900656]\n",
      " [-0.3821861   1.87031598  5.70374919]\n",
      " [-0.44356389  1.33601071  6.19313476]\n",
      " [ 2.92458188  1.14297018  5.76251047]\n",
      " [-0.58407232  0.78650068  5.68593284]\n",
      " [-0.52269453  1.32156756  5.19614757]]\n",
      "sqP = \n",
      "[[1.4391644493551392, 0.0, 0.0], [-0.06203120401660759, 0.524655582820248, 0.0], [0.03977955775271476, 0.0032071886126923523, 0.5120947806420015]]\n",
      "m = \n",
      "[0.33232373 1.10571781 5.829952  ]\n",
      "P = \n",
      "[[ 2.06711921e+00 -6.52632802e-02  4.14834789e-02]\n",
      " [-6.52632802e-02  7.53870199e-02 -1.76628368e-03]\n",
      " [ 4.14834789e-02 -1.76628368e-03  6.13118680e-02]]\n",
      "num = \n",
      "327\n",
      "SENSOR 8 4.94083414559 -2.11537295171\n",
      "\n",
      "num = \n",
      "328\n",
      "SENSOR 9 4.91446231227 -0.887651756022\n",
      "\n",
      "num = \n",
      "329\n",
      "SENSOR 2 2.16197834316 2.69904984308\n",
      "\n",
      "num = \n",
      "330\n",
      "SENSOR 3 1.525427237 -0.711047076592\n",
      "\n",
      "num = \n",
      "331\n",
      "SENSOR 7 4.01530297276 -1.90341160103\n",
      "\n",
      "num = \n",
      "332\n",
      "ODOMETRY -0.00041677675012 0.0998935259811 -0.000185627166787\n",
      "\n",
      "sqP = \n",
      "[[1.437747965007107, 0.0, 0.0], [-0.04539271259939433, 0.27078870268397687, 0.0], [0.028853095217527314, -0.0016860504904952721, 0.24591975130437077]]\n",
      "sigma = \n",
      "[[ 0.33232373  1.10571781  5.829952  ]\n",
      " [ 3.20781966  1.10571781  5.829952  ]\n",
      " [ 0.2415383   1.64729522  5.829952  ]\n",
      " [ 0.39002992  1.10234571  6.3217915 ]\n",
      " [-2.5431722   1.10571781  5.829952  ]\n",
      " [ 0.42310915  0.56414041  5.829952  ]\n",
      " [ 0.27461754  1.10908992  5.3381125 ]]\n",
      "sigmax = \n",
      "[[ 0.33193262  1.20021736  5.86254561]\n",
      " [-3.07575676  1.00597315  5.82335585]\n",
      " [ 0.24114719  1.74436056  5.85384398]\n",
      " [ 0.38963881  1.19480815  6.35978104]\n",
      " [-2.54356331  1.02311528  5.77365523]\n",
      " [ 0.42271804  0.65529582  5.87097878]\n",
      " [ 0.27422643  1.20531197  5.36520167]]\n",
      "sqP = \n",
      "[[1.4391475571460461, 0.0, 0.0], [0.06767806766384873, 0.5253809372993854, 0.0], [0.03015756281400743, -0.003213637574949255, 0.5117075751915016]]\n",
      "m = \n",
      "[-0.4563441   1.11373333  5.88440017]\n",
      "P = \n",
      "[[ 2.06676214  0.06919663  0.030837  ]\n",
      " [ 0.06919663  0.06691671 -0.01095105]\n",
      " [ 0.030837   -0.01095105  0.07683709]]\n",
      "num = \n",
      "333\n",
      "SENSOR 8 4.9944171849 -2.22316600512\n",
      "\n",
      "num = \n",
      "334\n",
      "SENSOR 9 4.91770926354 -0.875068268808\n",
      "\n",
      "num = \n",
      "335\n",
      "SENSOR 2 2.37108525182 2.63893274328\n",
      "\n",
      "num = \n",
      "336\n",
      "SENSOR 3 1.22148996766 -0.779993952297\n",
      "\n",
      "num = \n",
      "337\n",
      "SENSOR 7 4.05942517061 -1.80602776182\n",
      "\n",
      "num = \n",
      "338\n",
      "ODOMETRY -0.000504187302557 0.099935062112 -0.000102208043102\n",
      "\n",
      "sqP = \n",
      "[[1.4376237829523828, 0.0, 0.0], [0.04813263859290002, 0.2541652223255861, 0.0], [0.021449981654622214, -0.04714845218472034, 0.2723123411094371]]\n",
      "sigma = \n",
      "[[-0.4563441   1.11373333  5.88440017]\n",
      " [ 2.41890347  1.11373333  5.88440017]\n",
      " [-0.36007882  1.62206378  5.88440017]\n",
      " [-0.41344413  1.01943643  6.42902485]\n",
      " [-3.33159166  1.11373333  5.88440017]\n",
      " [-0.55260937  0.60540289  5.88440017]\n",
      " [-0.49924406  1.20803024  5.33977548]]\n",
      "sigmax = \n",
      "[[-0.4569465   1.20338635  5.84034279]\n",
      " [ 2.41830106  1.03883775  5.95050137]\n",
      " [-0.36068122  1.71553636  5.84916392]\n",
      " [-0.41404654  1.11089644  6.38885294]\n",
      " [ 2.95099124  1.01564532  5.90330673]\n",
      " [-0.55321178  0.69040617  5.83192962]\n",
      " [-0.49984646  1.29571129  5.29191371]]\n",
      "sqP = \n",
      "[[1.4391186901185875, 0.0, 0.0], [-0.06468391146006228, 0.5191067055617948, 0.0], [0.03853694446738703, -0.022844678785167806, 0.5241734418218763]]\n",
      "m = \n",
      "[0.34374042 1.13584096 6.04379022]\n",
      "P = \n",
      "[[ 2.0669518  -0.06731532  0.04225024]\n",
      " [-0.06731532  0.06596025 -0.01593245]\n",
      " [ 0.04225024 -0.01593245  0.07987509]]\n",
      "num = \n",
      "339\n",
      "SENSOR 9 5.05113586384 -0.939362710755\n",
      "\n",
      "num = \n",
      "340\n",
      "SENSOR 2 2.42075573102 2.65401889272\n",
      "\n",
      "num = \n",
      "341\n",
      "SENSOR 3 1.40673857311 -0.792218436955\n",
      "\n",
      "num = \n",
      "342\n",
      "SENSOR 7 4.01097220078 -1.86268980877\n",
      "\n",
      "num = \n",
      "343\n",
      "ODOMETRY 0.00010208423308 0.10009595201 0.000112598536447\n",
      "\n",
      "sqP = \n",
      "[[1.4376897456818611, 0.0, 0.0], [-0.046821866645281915, 0.252523187676179, 0.0], [0.029387591650312888, -0.057644073768964764, 0.2751156500565692]]\n",
      "sigma = \n",
      "[[ 0.34374042  1.13584096  6.04379022]\n",
      " [ 3.21911992  1.13584096  6.04379022]\n",
      " [ 0.25009669  1.64088734  6.04379022]\n",
      " [ 0.40251561  1.02055281  6.59402152]\n",
      " [-2.53163907  1.13584096  6.04379022]\n",
      " [ 0.43738416  0.63079459  6.04379022]\n",
      " [ 0.28496524  1.25112911  5.49355892]]\n",
      "sigmax = \n",
      "[[ 0.34313403  1.22994686  6.077422  ]\n",
      " [-3.06467179  1.03620219  6.03610052]\n",
      " [ 0.2494903   1.73772573  6.06847509]\n",
      " [ 0.40190921  1.11252064  6.63312313]\n",
      " [-2.53224546  1.05389788  5.98658579]\n",
      " [ 0.43677776  0.72134337  6.0860742 ]\n",
      " [ 0.28435885  1.34704809  5.52160472]]\n",
      "sqP = \n",
      "[[1.4392095416856465, 0.0, 0.0], [0.06654272996364351, 0.5196062696299437, 0.0], [0.031689513612554385, -0.0332712258693925, 0.525447879166371]]\n",
      "m = \n",
      "[-0.4523247   1.1275221   6.01873201]\n",
      "P = \n",
      "[[ 2.06715257  0.07079933  0.03137438]\n",
      " [ 0.07079933  0.09023108 -0.03537636]\n",
      " [ 0.03137438 -0.03537636  0.10797563]]\n",
      "num = \n",
      "344\n",
      "SENSOR 9 4.80175052631 -0.901315690154\n",
      "\n",
      "num = \n",
      "345\n",
      "SENSOR 2 2.53903797695 2.78425180019\n",
      "\n",
      "num = \n",
      "346\n",
      "SENSOR 3 1.21005538358 -0.947107299591\n",
      "\n",
      "num = \n",
      "347\n",
      "SENSOR 7 4.21526740137 -1.77072934034\n",
      "\n",
      "num = \n",
      "348\n",
      "ODOMETRY -0.000470079132191 0.100071592578 0.000364741934559\n",
      "\n",
      "sqP = \n",
      "[[1.4377595664916993, 0.0, 0.0], [0.04924281500521074, 0.2963211541198096, 0.0], [0.021821714647590012, -0.12301153499459053, 0.30392038890953743]]\n",
      "sigma = \n",
      "[[-0.4523247   1.1275221   6.01873201]\n",
      " [ 2.42319443  1.1275221   6.01873201]\n",
      " [-0.35383907  1.7201644   6.01873201]\n",
      " [-0.40868127  0.88149903  6.62657279]\n",
      " [-3.32784383  1.1275221   6.01873201]\n",
      " [-0.55081033  0.53487979  6.01873201]\n",
      " [-0.49596813  1.37354517  5.41089123]]\n",
      "sigmax = \n",
      "[[-0.45211002  1.21755621  5.9749935 ]\n",
      " [ 2.42340912  1.05215704  6.08460543]\n",
      " [-0.35362439  1.81406289  5.98405819]\n",
      " [-0.40846659  0.9733557   6.58680408]\n",
      " [ 2.95555616  1.02915538  6.03725736]\n",
      " [-0.55059565  0.62017697  5.96635271]\n",
      " [-0.49575345  1.46158526  5.36326622]]\n",
      "sqP = \n",
      "[[1.4391492788644349, 0.0, 0.0], [-0.06450418347835107, 0.5529118830637507, 0.0], [0.03883539838022074, -0.06562720415137717, 0.5378586489431758]]\n",
      "m = \n",
      "[0.35226519 1.11108305 6.19889477]\n",
      "P = \n",
      "[[ 2.0676936  -0.0679291   0.04636929]\n",
      " [-0.0679291   0.09281714 -0.04603972]\n",
      " [ 0.04636929 -0.04603972  0.11628258]]\n",
      "num = \n",
      "349\n",
      "SENSOR 9 4.8930608011 -0.931458155021\n",
      "\n",
      "num = \n",
      "350\n",
      "SENSOR 2 2.49064672217 2.70820827557\n",
      "\n",
      "num = \n",
      "351\n",
      "SENSOR 3 1.1298112679 -0.927638881263\n",
      "\n",
      "num = \n",
      "352\n",
      "SENSOR 7 4.10863769033 -1.85570810456\n",
      "\n",
      "num = \n",
      "353\n",
      "ODOMETRY 0.000293614872845 0.0999543517986 0.000235684154045\n",
      "\n",
      "sqP = \n",
      "[[1.437947705859565, 0.0, 0.0], [-0.04724031476933484, 0.3009742363728618, 0.0], [0.03224685732287104, -0.14790757198420665, 0.3055586148765979]]\n",
      "sigma = \n",
      "[[ 0.35226519  1.11108305  6.19889477]\n",
      " [ 3.22816061  1.11108305  6.19889477]\n",
      " [ 0.25778456  1.71303152  6.19889477]\n",
      " [ 0.41675891  0.8152679   6.810012  ]\n",
      " [-2.52363022  1.11108305  6.19889477]\n",
      " [ 0.44674582  0.50913457  6.19889477]\n",
      " [ 0.28777148  1.40689819  5.58777754]]\n",
      "sigmax = \n",
      "[[ 0.35215986  1.20502582  6.23337779]\n",
      " [-3.05513004  1.01138213  6.19028946]\n",
      " [ 0.25767923  1.80980844  6.22436143]\n",
      " [ 0.41665357  0.90679297  6.85047785]\n",
      " [-2.52373556  1.02949138  6.14095408]\n",
      " [ 0.44664049  0.59940523  6.24208657]\n",
      " [ 0.28766614  1.50286805  5.61613436]]\n",
      "sqP = \n",
      "[[1.439260604767386, 0.0, 0.0], [0.06261021847394231, 0.5623600832521553, 0.0], [0.03362076686846698, -0.08383768319736405, 0.536746469916953]]\n",
      "m = \n",
      "[-0.43653797  1.12910409  6.21635963]\n",
      "P = \n",
      "[[ 2.06756329  0.06470035  0.03491186]\n",
      " [ 0.06470035  0.0879241  -0.0394834 ]\n",
      " [ 0.03491186 -0.0394834   0.12011724]]\n",
      "num = \n",
      "354\n",
      "SENSOR 9 4.78120287936 -0.931491845794\n",
      "\n",
      "num = \n",
      "355\n",
      "SENSOR 2 2.65158902922 2.74003752891\n",
      "\n",
      "num = \n",
      "356\n",
      "SENSOR 3 1.13696167911 -1.02091756573\n",
      "\n",
      "num = \n",
      "357\n",
      "SENSOR 7 4.14832981289 -1.93565577604\n",
      "\n",
      "num = \n",
      "358\n",
      "ODOMETRY -8.40851246277e-05 0.0999403556225 -0.000176595962094\n",
      "\n",
      "sqP = \n",
      "[[1.4379023915564115, 0.0, 0.0], [0.044996340653670225, 0.2930860490048228, 0.0], [0.02427971550645465, -0.1384436380103579, 0.3167981997467523]]\n",
      "sigma = \n",
      "[[-0.43653797  1.12910409  6.21635963]\n",
      " [ 2.43926682  1.12910409  6.21635963]\n",
      " [-0.34654528  1.71527619  6.21635963]\n",
      " [-0.38797853  0.85221681  6.84995603]\n",
      " [-3.31234275  1.12910409  6.21635963]\n",
      " [-0.52653065  0.54293199  6.21635963]\n",
      " [-0.4850974   1.40599137  5.58276323]]\n",
      "sigmax = \n",
      "[[-0.43600867  1.21969721  6.17412507]\n",
      " [ 2.43979612  1.0527858   6.28090722]\n",
      " [-0.34601598  1.80929839  6.18243769]\n",
      " [-0.38744924  0.94475323  6.81216867]\n",
      " [ 2.97137186  1.03059833  6.23331511]\n",
      " [-0.52600135  0.62936285  6.16615426]\n",
      " [-0.4845681   1.49442762  5.53618103]]\n",
      "sqP = \n",
      "[[1.4390364131892983, 0.0, 0.0], [-0.06640636053860444, 0.5544825603230926, 0.0], [0.03834973689004591, -0.07716644533071426, 0.5438573083640806]]\n",
      "m = \n",
      "[0.35838907 1.14816434 6.30285291]\n",
      "P = \n",
      "[[ 2.06708136 -0.06878806  0.04513425]\n",
      " [-0.06878806  0.08928005 -0.04316232]\n",
      " [ 0.04513425 -0.04316232  0.1202457 ]]\n",
      "num = \n",
      "359\n",
      "SENSOR 9 4.57621416752 -0.993635910976\n",
      "\n",
      "num = \n",
      "360\n",
      "SENSOR 2 2.92434868722 2.7082657163\n",
      "\n",
      "num = \n",
      "361\n",
      "SENSOR 3 0.871667628237 -1.08932996938\n",
      "\n",
      "num = \n",
      "362\n",
      "SENSOR 7 4.2336588264 -1.9442154823\n",
      "\n",
      "num = \n",
      "363\n",
      "ODOMETRY -0.00962491243249 0.100226987653 -0.0205912870107\n",
      "\n",
      "sqP = \n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[1.4377348002608838, 0.0, 0.0], [-0.04784474768947098, 0.29494224509869826, 0.0], [0.03139260931233215, -0.1412491793555931, 0.3151330984707076]]\n",
      "sigma = \n",
      "[[ 0.35838907  1.14816434  6.30285291]\n",
      " [ 3.23385867  1.14816434  6.30285291]\n",
      " [ 0.26269958  1.73804883  6.30285291]\n",
      " [ 0.42117429  0.86566598  6.9331191 ]\n",
      " [-2.51708053  1.14816434  6.30285291]\n",
      " [ 0.45407857  0.55827985  6.30285291]\n",
      " [ 0.29560385  1.4306627   5.67258671]]\n",
      "sigmax = \n",
      "[[ 0.35812839  1.24175774  6.33790073]\n",
      " [-3.04958732  1.04864831  6.29365325]\n",
      " [ 0.2624389   1.83456266  6.32879815]\n",
      " [ 0.42091361  0.95687593  6.97397029]\n",
      " [-2.51734121  1.06708297  6.24442444]\n",
      " [ 0.45381789  0.64809649  6.34668264]\n",
      " [ 0.29534317  1.52627073  5.70169305]]\n",
      "sqP = \n",
      "[[1.4392813522201704, 0.0, 0.0], [0.06263827987058324, 0.5573766391546734, 0.0], [0.03409985421537736, -0.08329476617048767, 0.5423337282051598]]\n",
      "m = \n",
      "[-0.40783048  1.19923066  6.55815883]\n",
      "P = \n",
      "[[ 2.06762785  0.06515325  0.03543563]\n",
      " [ 0.06515325  0.08662968 -0.03658252]\n",
      " [ 0.03543563 -0.03658252  0.12116135]]\n",
      "num = \n",
      "364\n",
      "SENSOR 9 4.44837110071 -0.989307488326\n",
      "\n",
      "num = \n",
      "365\n",
      "SENSOR 2 2.8892743732 2.84845502425\n",
      "\n",
      "num = \n",
      "366\n",
      "SENSOR 3 0.891192383698 -1.23984271\n",
      "\n",
      "num = \n",
      "367\n",
      "SENSOR 7 4.24686986677 -1.92323203717\n",
      "\n",
      "num = \n",
      "368\n",
      "ODOMETRY -0.0102033452948 0.0998839630755 -0.0215571904424\n",
      "\n",
      "sqP = \n",
      "[[1.4379248418365018, 0.0, 0.0], [0.04531060750135832, 0.2908206086526517, 0.0], [0.024643592630376868, -0.12963021083584103, 0.32210254762655705]]\n",
      "sigma = \n",
      "[[-0.40783048  1.19923066  6.55815883]\n",
      " [ 2.4680192   1.19923066  6.55815883]\n",
      " [-0.31720926  1.78087188  6.55815883]\n",
      " [-0.35854329  0.93997024  7.20236393]\n",
      " [-3.28368016  1.19923066  6.55815883]\n",
      " [-0.49845169  0.61758944  6.55815883]\n",
      " [-0.45711766  1.45849108  5.91395374]]\n",
      "sigmax = \n",
      "[[-0.43804668  1.29085051  6.51752323]\n",
      " [ 2.437803    1.12149885  6.6214298 ]\n",
      " [-0.34742546  1.87579319  6.52598132]\n",
      " [-0.38875949  1.03348083  7.16629153]\n",
      " [ 2.96928894  1.1001549   6.57330625]\n",
      " [-0.52866789  0.70515594  6.50939863]\n",
      " [-0.48733386  1.54799767  5.86885363]]\n",
      "sqP = \n",
      "[[1.4390503863760913, 0.0, 0.0], [-0.06710350521402818, 0.5511680276443501, 0.0], [0.037227812696646885, -0.07372848451158422, 0.5474873470412008]]\n",
      "m = \n",
      "[0.35430882 1.22727916 6.62884989]\n",
      "P = \n",
      "[[ 2.06696331 -0.06901445  0.04294435]\n",
      " [-0.06901445  0.0855461  -0.03475532]\n",
      " [ 0.04294435 -0.03475532  0.12036745]]\n",
      "num = \n",
      "369\n",
      "SENSOR 9 4.51127976731 -0.965675234448\n",
      "\n",
      "num = \n",
      "370\n",
      "SENSOR 2 2.99371450745 2.84262811823\n",
      "\n",
      "num = \n",
      "371\n",
      "SENSOR 3 0.799137718613 -1.19111999767\n",
      "\n",
      "num = \n",
      "372\n",
      "SENSOR 7 4.34718168559 -1.87200596541\n",
      "\n",
      "num = \n",
      "373\n",
      "ODOMETRY -0.00860842707999 0.0998102570683 -0.0193391675193\n",
      "\n",
      "sqP = \n",
      "[[1.4376937468010844, 0.0, 0.0], [-0.04800358147775222, 0.28851647284750714, 0.0], [0.029870303017450847, -0.11549231101105888, 0.32578634960085173]]\n",
      "sigma = \n",
      "[[ 0.35430882  1.22727916  6.62884989]\n",
      " [ 3.22969632  1.22727916  6.62884989]\n",
      " [ 0.25830166  1.80431211  6.62884989]\n",
      " [ 0.41404943  0.99629454  7.28042259]\n",
      " [-2.52107867  1.22727916  6.62884989]\n",
      " [ 0.45031599  0.65024621  6.62884989]\n",
      " [ 0.29456822  1.45826378  5.97727719]]\n",
      "sigmax = \n",
      "[[ 0.32254829  1.32130769  6.66254622]\n",
      " [-3.08524952  1.12769812  6.62107677]\n",
      " [ 0.22654113  1.90113774  6.65337749]\n",
      " [ 0.3822889   1.08814348  7.31967279]\n",
      " [-2.5528392   1.14542725  6.57160419]\n",
      " [ 0.41855545  0.7406116   6.67140459]\n",
      " [ 0.26280768  1.55413641  6.00529943]]\n",
      "sqP = \n",
      "[[1.4392580383108198, 0.0, 0.0], [0.0638444170838358, 0.5477931614320266, 0.0], [0.0328929346955492, -0.07202456517005286, 0.5501794745359516]]\n",
      "m = \n",
      "[-0.4660322   1.18897669  6.73709099]\n",
      "P = \n",
      "[[ 2.06759207  0.06702998  0.03435693]\n",
      " [ 0.06702998  0.08420614 -0.02701514]\n",
      " [ 0.03435693 -0.02701514  0.11988236]]\n",
      "num = \n",
      "374\n",
      "SENSOR 9 4.45640142154 -0.978412228293\n",
      "\n",
      "num = \n",
      "375\n",
      "SENSOR 2 3.26617603062 2.8384001977\n",
      "\n",
      "num = \n",
      "376\n",
      "SENSOR 3 0.816222604143 -1.30597066763\n",
      "\n",
      "num = \n",
      "377\n",
      "SENSOR 7 4.2980455787 -1.89648658794\n",
      "\n",
      "num = \n",
      "378\n",
      "ODOMETRY -0.00956022288558 0.100150972292 -0.0200463224548\n",
      "\n",
      "sqP = \n",
      "[[1.4379124003616106, 0.0, 0.0], [0.04661618116833797, 0.2864141642015731, 0.0], [0.02389361835503237, -0.09821080924080708, 0.33115870590329843]]\n",
      "sigma = \n",
      "[[-0.4660322   1.18897669  6.73709099]\n",
      " [ 2.4097926   1.18897669  6.73709099]\n",
      " [-0.37279984  1.76180502  6.73709099]\n",
      " [-0.41824497  0.99255507  7.39940841]\n",
      " [-3.341857    1.18897669  6.73709099]\n",
      " [-0.55926457  0.61614836  6.73709099]\n",
      " [-0.51381944  1.38539831  6.07477358]]\n",
      "sigmax = \n",
      "[[-0.4939798   1.27775363  6.69147583]\n",
      " [ 2.381845    1.11529745  6.80442195]\n",
      " [-0.40074744  1.85444305  6.69993883]\n",
      " [-0.44619256  1.08340965  7.3580861 ]\n",
      " [ 2.91338071  1.09133578  6.75778738]\n",
      " [-0.58721216  0.70029309  6.68340904]\n",
      " [-0.54176703  1.4718949   6.0249697 ]]\n",
      "sqP = \n",
      "[[1.4390805871991312, 0.0, 0.0], [-0.06415371744867875, 0.542495462438138, 0.0], [0.04124387262164857, -0.05785558446445545, 0.5546918848671993]]\n",
      "m = \n",
      "[0.3016217  1.22900753 6.86889957]\n",
      "P = \n",
      "[[ 2.06710679 -0.06649384  0.04658719]\n",
      " [-0.06649384  0.08313826 -0.02806042]\n",
      " [ 0.04658719 -0.02806042  0.1182809 ]]\n",
      "num = \n",
      "379\n",
      "SENSOR 9 4.37573768277 -0.917844889679\n",
      "\n",
      "num = \n",
      "380\n",
      "SENSOR 2 3.317865598 2.95007305006\n",
      "\n",
      "num = \n",
      "381\n",
      "SENSOR 3 0.817247142217 -1.37869382939\n",
      "\n",
      "num = \n",
      "382\n",
      "SENSOR 7 4.09119325406 -1.94192716645\n",
      "\n",
      "num = \n",
      "383\n",
      "ODOMETRY -0.0103002834124 0.100007766723 -0.0197368000329\n",
      "\n",
      "sqP = \n",
      "[[1.4377436442364144, 0.0, 0.0], [-0.046248742470472816, 0.2846037759731119, 0.0], [0.03240298900098716, -0.09332913748175187, 0.3294246851118377]]\n",
      "sigma = \n",
      "[[ 0.3016217   1.22900753  6.86889957]\n",
      " [ 3.17710899  1.22900753  6.86889957]\n",
      " [ 0.20912422  1.79821508  6.86889957]\n",
      " [ 0.36642768  1.04234925  7.52774895]\n",
      " [-2.57386558  1.22900753  6.86889957]\n",
      " [ 0.39411919  0.65979998  6.86889957]\n",
      " [ 0.23681573  1.4156658   6.2100502 ]]\n",
      "sigmax = \n",
      "[[ 0.27201516  1.32491734  6.89773575]\n",
      " [-3.13568286  1.12889029  6.86630034]\n",
      " [ 0.17951767  1.89637837  6.8887537 ]\n",
      " [ 0.33682114  1.13619029  7.56273576]\n",
      " [-2.60347213  1.14405673  6.8158564 ]\n",
      " [ 0.36451264  0.75263632  6.90647125]\n",
      " [ 0.20720918  1.51324173  6.23261466]]\n",
      "sqP = \n",
      "[[1.4392507569623025, 0.0, 0.0], [0.06608193422677888, 0.541235728749862, 0.0], [0.029626361975473324, -0.06026910064528934, 0.5539364053126348]]\n",
      "m = \n",
      "[-0.50539184  1.34133464  6.88676134]\n",
      "P = \n",
      "[[ 2.06755514  0.0696231   0.03101766]\n",
      " [ 0.0696231   0.08226185 -0.01912987]\n",
      " [ 0.03101766 -0.01912987  0.11619873]]\n",
      "num = \n",
      "384\n",
      "SENSOR 9 4.21811509958 -0.851878267922\n",
      "\n",
      "num = \n",
      "385\n",
      "SENSOR 2 3.18846120949 2.97202181201\n",
      "\n",
      "num = \n",
      "386\n",
      "SENSOR 3 1.13340957174 -1.46851281333\n",
      "\n",
      "num = \n",
      "387\n",
      "SENSOR 7 4.38585468217 -1.82603901749\n",
      "\n",
      "num = \n",
      "388\n",
      "ODOMETRY -0.0102831048513 0.10010953823 -0.0202530209795\n",
      "\n",
      "sqP = \n",
      "[[1.4378995578662994, 0.0, 0.0], [0.04842000284889137, 0.2826965796218238, 0.0], [0.02157150608609372, -0.07136401937939187, 0.3326267817794668]]\n",
      "sigma = \n",
      "[[-0.50539184  1.34133464  6.88676134]\n",
      " [ 2.37040728  1.34133464  6.88676134]\n",
      " [-0.40855183  1.9067278   6.88676134]\n",
      " [-0.46224883  1.1986066   7.55201491]\n",
      " [-3.38119095  1.34133464  6.88676134]\n",
      " [-0.60223184  0.77594148  6.88676134]\n",
      " [-0.54853485  1.48406268  6.22150778]]\n",
      "sigmax = \n",
      "[[-0.53542892  1.42833655  6.83744381]\n",
      " [ 2.34037019  1.27034233  6.9572003 ]\n",
      " [-0.43858892  1.99809053  6.84608698]\n",
      " [-0.49228591  1.28765461  7.50649562]\n",
      " [ 2.87195727  1.24443336  6.91149383]\n",
      " [-0.63226893  0.85776731  6.82926277]\n",
      " [-0.57857193  1.56885659  6.16848377]]\n",
      "sqP = \n",
      "[[1.439104653229439, 0.0, 0.0], [-0.061965504827998445, 0.5365486393359993, 0.0], [0.04372661837306914, -0.042182999702831084, 0.5568719709384895]]\n",
      "m = \n",
      "[0.27203178 1.24211372 6.94064587]\n",
      "P = \n",
      "[[ 2.067306   -0.06538252  0.04904728]\n",
      " [-0.06538252  0.08619646 -0.02453841]\n",
      " [ 0.04904728 -0.02453841  0.11620147]]\n",
      "num = \n",
      "389\n",
      "SENSOR 9 4.26699097617 -0.944178359109\n",
      "\n",
      "num = \n",
      "390\n",
      "SENSOR 2 3.38023967491 3.01452503314\n",
      "\n",
      "num = \n",
      "391\n",
      "SENSOR 3 0.720668136763 -1.55711368901\n",
      "\n",
      "num = \n",
      "392\n",
      "SENSOR 7 4.33394842239 -1.91875382264\n",
      "\n",
      "num = \n",
      "393\n",
      "ODOMETRY -0.0105866501206 0.0999017879145 -0.0197382893367\n",
      "\n",
      "sqP = \n",
      "[[1.437812922195286, 0.0, 0.0], [-0.04547359245960688, 0.29004932709984477, 0.0], [0.034112423500233406, -0.07925271269208271, 0.3297829873407184]]\n",
      "sigma = \n",
      "[[ 0.27203178  1.24211372  6.94064587]\n",
      " [ 3.14765763  1.24211372  6.94064587]\n",
      " [ 0.1810846   1.82221237  6.94064587]\n",
      " [ 0.34025663  1.08360829  7.60021184]\n",
      " [-2.60359406  1.24211372  6.94064587]\n",
      " [ 0.36297897  0.66201506  6.94064587]\n",
      " [ 0.20380694  1.40061914  6.28107989]]\n",
      "sigmax = \n",
      "[[ 0.24149566  1.33881342  6.96655122]\n",
      " [ 3.1171215   1.14200507  6.94106814]\n",
      " [ 0.15054847  1.9208652   6.95766171]\n",
      " [ 0.3097205   1.17831701  7.63264913]\n",
      " [-2.63413019  1.15562311  6.89023461]\n",
      " [ 0.33244284  0.75596235  6.9752266 ]\n",
      " [ 0.17327081  1.4988599   6.30033277]]\n",
      "sqP = \n",
      "[[1.5068302838342444, 0.0, 0.0], [-0.013851428113348899, 0.5457176693241422, 0.0], [0.01979924028189175, -0.04751551214959953, 0.5558394044131163]]\n",
      "m = \n",
      "[0.24405655 1.30031061 7.03728704]\n",
      "P = \n",
      "[[ 2.27016311 -0.01509176  0.02302746]\n",
      " [-0.01509176  0.07959258 -0.01913783]\n",
      " [ 0.02302746 -0.01913783  0.11348651]]\n",
      "num = \n",
      "394\n",
      "SENSOR 9 4.22270883023 -0.860892437552\n",
      "\n",
      "num = \n",
      "395\n",
      "SENSOR 2 3.36042267996 3.01995506444\n",
      "\n",
      "num = \n",
      "396\n",
      "SENSOR 3 0.717148685032 -1.66529431909\n",
      "\n",
      "num = \n",
      "397\n",
      "SENSOR 7 4.43948581701 -1.84649833616\n",
      "\n",
      "num = \n",
      "398\n",
      "ODOMETRY -0.0113245534652 0.100123041114 -0.0190696769167\n",
      "\n",
      "sqP = \n",
      "[[1.5067060462727175, 0.0, 0.0], [-0.010016390549907465, 0.28194370390167184, 0.0], [0.015283315962421795, -0.06733522429643925, 0.3297254908194877]]\n",
      "sigma = \n",
      "[[ 0.24405655  1.30031061  7.03728704]\n",
      " [ 3.25746864  1.30031061  7.03728704]\n",
      " [ 0.22402377  1.86419802  7.03728704]\n",
      " [ 0.27462318  1.16564016  7.69673803]\n",
      " [-2.76935554  1.30031061  7.03728704]\n",
      " [ 0.26408933  0.7364232   7.03728704]\n",
      " [ 0.21348992  1.43498106  6.37783606]]\n",
      "sigmax = \n",
      "[[ 0.21373161  1.39750201  7.06039979]\n",
      " [-3.0560416   1.20096206  7.02678788]\n",
      " [ 0.19369883  1.9618329   7.05844827]\n",
      " [ 0.24429824  1.26207979  7.72281033]\n",
      " [-2.79968048  1.20687106  7.00193995]\n",
      " [ 0.23376439  0.83313212  7.06234204]\n",
      " [ 0.18316498  1.53283343  6.39796766]]\n",
      "sqP = \n",
      "[[1.4335244319815834, 0.0, 0.0], [0.07690995390891191, 0.5341504059648532, 0.0], [0.022174689239175538, -0.04256821953656698, 0.5548274974938294]]\n",
      "m = \n",
      "[-0.57875096  1.28247374  7.09803625]\n",
      "P = \n",
      "[[ 2.05030859  0.08097376  0.02261353]\n",
      " [ 0.08097376  0.08554953 -0.01443137]\n",
      " [ 0.02261353 -0.01443137  0.11166428]]\n",
      "num = \n",
      "399\n",
      "SENSOR 9 4.16843098657 -0.834433300637\n",
      "\n",
      "num = \n",
      "400\n",
      "SENSOR 2 3.51256815216 2.97921133252\n",
      "\n",
      "num = \n",
      "401\n",
      "SENSOR 3 0.834830514938 -1.73903910734\n",
      "\n",
      "num = \n",
      "402\n",
      "SENSOR 7 4.28359501376 -1.86953074931\n",
      "\n",
      "num = \n",
      "403\n",
      "ODOMETRY -0.00971082754926 0.100025950554 -0.0198211377492\n",
      "\n",
      "sqP = \n",
      "[[1.4318898655126797, 0.0, 0.0], [0.05655027160147829, 0.28696967194866707, 0.0], [0.01579278651203134, -0.05340094839880369, 0.3294893148622617]]\n",
      "sigma = \n",
      "[[-0.57875096  1.28247374  7.09803625]\n",
      " [ 2.28502878  1.28247374  7.09803625]\n",
      " [-0.46565041  1.85641309  7.09803625]\n",
      " [-0.54716538  1.17567185  7.75701488]\n",
      " [-3.44253069  1.28247374  7.09803625]\n",
      " [-0.6918515   0.7085344   7.09803625]\n",
      " [-0.61033653  1.38927564  6.43905762]]\n",
      "sigmax = \n",
      "[[-0.60914519  1.36566584  7.04232541]\n",
      " [ 2.25463455  1.2177503   7.17442669]\n",
      " [-0.49604464  1.94536117  7.05207037]\n",
      " [-0.57755961  1.26058182  7.70395906]\n",
      " [ 2.81026039  1.18719256  7.12879531]\n",
      " [-0.72224573  0.78490748  7.03329232]\n",
      " [-0.64073076  1.47066688  6.38074733]]\n",
      "sqP = \n",
      "[[1.439893459778958, 0.0, 0.0], [-0.056602496749835, 0.5373646241216972, 0.0], [0.04736762507764246, -0.0308546599328415, 0.5555368681321345]]\n",
      "m = \n",
      "[0.17912159 1.34731919 7.13583459]\n",
      "P = \n",
      "[[ 2.06968585 -0.05935951  0.05214898]\n",
      " [-0.05935951  0.08304679 -0.01754858]\n",
      " [ 0.05214898 -0.01754858  0.11191667]]\n",
      "num = \n",
      "404\n",
      "SENSOR 9 4.02856295399 -0.968273518321\n",
      "\n",
      "num = \n",
      "405\n",
      "SENSOR 2 3.63658640677 3.12915861055\n",
      "\n",
      "num = \n",
      "406\n",
      "SENSOR 3 0.734021188267 -1.75470305372\n",
      "\n",
      "num = \n",
      "407\n",
      "SENSOR 7 4.54650380966 -1.76609368488\n",
      "\n",
      "num = \n",
      "408\n",
      "ODOMETRY -0.010589475487 0.10010496938 -0.0199922914546\n",
      "\n",
      "sqP = \n",
      "[[1.4386402779794003, 0.0, 0.0], [-0.04126083995366435, 0.285209280946063, 0.0], [0.03624879576674533, -0.056284735593472086, 0.3277723655763295]]\n",
      "sigma = \n",
      "[[ 0.17912159  1.34731919  7.13583459]\n",
      " [ 3.05640214  1.34731919  7.13583459]\n",
      " [ 0.09659991  1.91773775  7.13583459]\n",
      " [ 0.25161918  1.23474972  7.79137932]\n",
      " [-2.69815897  1.34731919  7.13583459]\n",
      " [ 0.26164327  0.77690063  7.13583459]\n",
      " [ 0.106624    1.45988866  6.48028986]]\n",
      "sigmax = \n",
      "[[ 0.14958962  1.4459132   7.15269912]\n",
      " [ 3.02687018  1.24774333  7.14531294]\n",
      " [ 0.06706794  2.01738636  7.14451482]\n",
      " [ 0.22208721  1.33186317  7.81534112]\n",
      " [-2.72769093  1.25655489  7.09379849]\n",
      " [ 0.2321113   0.87376901  7.16076864]\n",
      " [ 0.07709203  1.55944525  6.48996852]]\n",
      "sqP = \n",
      "[[1.5075484342245418, 0.0, 0.0], [-0.011295301121469413, 0.5402600113660464, 0.0], [0.020368261551911477, -0.034447003799722965, 0.5554950372104474]]\n",
      "m = \n",
      "[0.15703934 1.3364242  7.31901089]\n",
      "P = \n",
      "[[ 2.27235312 -0.01248699  0.02351889]\n",
      " [-0.01248699  0.08205324 -0.01455281]\n",
      " [ 0.02351889 -0.01455281  0.10951098]]\n",
      "num = \n",
      "409\n",
      "SENSOR 9 3.91015885795 -0.869086963006\n",
      "\n",
      "num = \n",
      "410\n",
      "SENSOR 2 3.87255632648 3.16604194771\n",
      "\n",
      "num = \n",
      "411\n",
      "SENSOR 3 0.865196059921 -1.86949836389\n",
      "\n",
      "num = \n",
      "412\n",
      "SENSOR 7 4.51133867781 -1.81430214087\n",
      "\n",
      "num = \n",
      "413\n",
      "ODOMETRY -0.00946128189441 0.0997349532468 -0.0199718063448\n",
      "\n",
      "sqP = \n",
      "[[1.5074326259644406, 0.0, 0.0], [-0.008283615891346312, 0.2863295759799053, 0.0], [0.015601953341925073, -0.05037401366886337, 0.3266955981924475]]\n",
      "sigma = \n",
      "[[ 0.15703934  1.3364242   7.31901089]\n",
      " [ 3.17190459  1.3364242   7.31901089]\n",
      " [ 0.14047211  1.90908336  7.31901089]\n",
      " [ 0.18824324  1.23567618  7.97240209]\n",
      " [-2.85782591  1.3364242   7.31901089]\n",
      " [ 0.17360657  0.76376505  7.31901089]\n",
      " [ 0.12583543  1.43717223  6.66561969]]\n",
      "sigmax = \n",
      "[[ 0.12645757  1.43545759  7.3336189 ]\n",
      " [ 3.14132282  1.2363387   7.3170367 ]\n",
      " [ 0.10989034  2.00834515  7.33197626]\n",
      " [ 0.15766148  1.3342056   7.99009271]\n",
      " [-2.88840768  1.24003127  7.29200335]\n",
      " [ 0.1430248   0.86254284  7.33525753]\n",
      " [ 0.09525366  1.53661316  6.67713087]]\n",
      "sqP = \n",
      "[[1.5724710366370402, 0.0, 0.0], [-0.002896021110829568, 0.5402798035507588, 0.0], [0.009260591419898958, -0.02914679412297515, 0.5540803892171401]]\n",
      "m = \n",
      "[0.1291184  1.39931486 7.49257995]\n",
      "P = \n",
      "[[ 2.47260302 -0.00331758  0.01118363]\n",
      " [-0.00331758  0.08145565 -0.00918664]\n",
      " [ 0.01118363 -0.00918664  0.10448502]]\n",
      "num = \n",
      "414\n",
      "SENSOR 9 3.82613593432 -0.874876680077\n",
      "\n",
      "num = \n",
      "415\n",
      "SENSOR 2 3.83879660579 -3.13229830775\n",
      "\n",
      "num = \n",
      "416\n",
      "SENSOR 3 1.06514315073 -1.87293500873\n",
      "\n",
      "num = \n",
      "417\n",
      "SENSOR 7 4.56299343999 -1.75934253741\n",
      "\n",
      "num = \n",
      "418\n",
      "ODOMETRY -0.00979951516687 0.0997189457445 -0.0206080614093\n",
      "\n",
      "sqP = \n",
      "[[1.5724512781793294, 0.0, 0.0], [-0.0021098136949756936, 0.2853965700770542, 0.0], [0.007112226267275715, -0.0321364632468925, 0.3215613262566296]]\n",
      "sigma = \n",
      "[[ 0.1291184   1.39931486  7.49257995]\n",
      " [ 3.27402096  1.39931486  7.49257995]\n",
      " [ 0.12489877  1.970108    7.49257995]\n",
      " [ 0.14334285  1.33504193  8.13570261]\n",
      " [-3.01578415  1.39931486  7.49257995]\n",
      " [ 0.13333803  0.82852172  7.49257995]\n",
      " [ 0.11489395  1.46358779  6.8494573 ]]\n",
      "sigmax = \n",
      "[[ 0.09968531  1.49833667  7.50448549]\n",
      " [-3.03859744  1.300333    7.48034673]\n",
      " [ 0.09546569  2.06917917  7.50406755]\n",
      " [ 0.11390977  1.43388438  8.14901542]\n",
      " [-3.04521724  1.30025419  7.48100223]\n",
      " [ 0.10390494  0.92749241  7.50490322]\n",
      " [ 0.08546086  1.56276892  6.85995315]]\n",
      "sqP = \n",
      "[[1.4319946225962734, 0.0, 0.0], [0.08088266759615113, 0.5322738162579957, 0.0], [0.011395142455018183, -0.019690844347466582, 0.5508574136176531]]\n",
      "m = \n",
      "[-0.69088904  1.40040322  7.58205916]\n",
      "P = \n",
      "[[ 2.04586882  0.08542047  0.0117302 ]\n",
      " [ 0.08542047  0.08890986 -0.00449421]\n",
      " [ 0.0117302  -0.00449421  0.09884444]]\n",
      "num = \n",
      "419\n",
      "SENSOR 9 4.12881342913 -0.867651605124\n",
      "\n",
      "num = \n",
      "420\n",
      "SENSOR 2 4.0288094419 -3.2276561161\n",
      "\n",
      "num = \n",
      "421\n",
      "SENSOR 3 0.901200006923 -2.03251122022\n",
      "\n",
      "num = \n",
      "422\n",
      "SENSOR 7 4.62001856502 -1.8309460386\n",
      "\n",
      "num = \n",
      "423\n",
      "ODOMETRY -0.00943323576827 0.100124414548 -0.0203296697597\n",
      "\n",
      "sqP = \n",
      "[[1.4303387096201792, 0.0, 0.0], [0.05972044898653225, 0.2921358060310251, 0.0], [0.008200995470410675, -0.017060474963892243, 0.3138249834085996]]\n",
      "sigma = \n",
      "[[-0.69088904  1.40040322  7.58205916]\n",
      " [ 2.16978838  1.40040322  7.58205916]\n",
      " [-0.57144815  1.98467483  7.58205916]\n",
      " [-0.67448705  1.36628227  8.20970913]\n",
      " [-3.55156646  1.40040322  7.58205916]\n",
      " [-0.81032994  0.8161316   7.58205916]\n",
      " [-0.70729103  1.43452417  6.95440919]]\n",
      "sigmax = \n",
      "[[-0.72129662  1.47662822  7.51776595]\n",
      " [ 2.1393808   1.34499041  7.66496446]\n",
      " [-0.60185572  2.06801775  7.52730676]\n",
      " [-0.70489463  1.44355151  8.14667475]\n",
      " [ 2.70121127  1.30934173  7.62269996]\n",
      " [-0.84073752  0.88415254  7.50914126]\n",
      " [-0.73769861  1.50968443  6.88887444]]\n",
      "sqP = \n",
      "[[1.4401086611018326, 0.0, 0.0], [-0.049748995316037506, 0.5381933957860495, 0.0], [0.05244982162953744, -0.00868742581202925, 0.5469526746598311]]\n",
      "m = \n",
      "[0.0815265  1.31963013 7.61695725]\n",
      "P = \n",
      "[[ 2.07032424 -0.05246712  0.05630079]\n",
      " [-0.05246712  0.08553373 -0.00643797]\n",
      " [ 0.05630079 -0.00643797  0.099134  ]]\n",
      "num = \n",
      "424\n",
      "SENSOR 9 3.70232136893 -0.94416511906\n",
      "\n",
      "num = \n",
      "425\n",
      "SENSOR 2 3.98368634906 -3.07682994555\n",
      "\n",
      "num = \n",
      "426\n",
      "SENSOR 3 0.899772694406 -2.09221030348\n",
      "\n",
      "num = \n",
      "427\n",
      "SENSOR 7 4.46139118814 -1.86311006595\n",
      "\n",
      "num = \n",
      "428\n",
      "ODOMETRY -0.0100890424076 0.0999779958111 -0.0198161117798\n",
      "\n",
      "sqP = \n",
      "[[1.4388621320972328, 0.0, 0.0], [-0.03646431100911486, 0.29017939202970755, 0.0], [0.03912869074616591, -0.017269218429696906, 0.3119370450193106]]\n",
      "sigma = \n",
      "[[ 8.15264982e-02  1.31963013e+00  7.61695725e+00]\n",
      " [ 2.95925076e+00  1.31963013e+00  7.61695725e+00]\n",
      " [ 8.59787616e-03  1.89998892e+00  7.61695725e+00]\n",
      " [ 1.59783880e-01  1.28509170e+00  8.24083134e+00]\n",
      " [-2.79619777e+00  1.31963013e+00  7.61695725e+00]\n",
      " [ 1.54455120e-01  7.39271350e-01  7.61695725e+00]\n",
      " [ 3.26911668e-03  1.35416857e+00  6.99308316e+00]]\n",
      "sigmax = \n",
      "[[ 0.05176359  1.41949447  7.62416929]\n",
      " [ 2.92948786  1.22134126  7.63604114]\n",
      " [-0.02116503  2.0001133   7.61687361]\n",
      " [ 0.13002097  1.38408657  8.25582846]\n",
      " [-2.82596067  1.2251033   7.58394851]\n",
      " [ 0.12469221  0.83834473  7.63142664]\n",
      " [-0.02649379  1.45429108  6.99246598]]\n",
      "sqP = \n",
      "[[1.507708504851078, 0.0, 0.0], [-0.008377476236255616, 0.5401892362426286, 0.0], [0.020713303819880225, -0.010955717925911124, 0.5471989270105322]]\n",
      "m = \n",
      "[0.05047052 1.50506222 7.64184795]\n",
      "P = \n",
      "[[ 2.27283989e+00 -9.19729364e-03  2.34573193e-02]\n",
      " [-9.19729364e-03  8.20268365e-02 -1.32130942e-03]\n",
      " [ 2.34573193e-02 -1.32130942e-03  9.49213096e-02]]\n",
      "num = \n",
      "429\n",
      "SENSOR 9 3.7518939771 -0.918292146011\n",
      "\n",
      "num = \n",
      "430\n",
      "SENSOR 2 4.21633761598 -3.02832665334\n",
      "\n",
      "num = \n",
      "431\n",
      "SENSOR 3 1.18763314519 -2.0979866523\n",
      "\n",
      "num = \n",
      "432\n",
      "SENSOR 7 4.57848442895 -1.89875432575\n",
      "\n",
      "num = \n",
      "433\n",
      "ODOMETRY -0.00985362326451 0.100067538531 -0.0193174898999\n",
      "\n",
      "sqP = \n",
      "[[1.5075940743931198, 0.0, 0.0], [-0.0061006432644217816, 0.2863382940654599, 0.0], [0.015559439838819786, -0.004282999691770111, 0.30767006568944977]]\n",
      "sigma = \n",
      "[[ 0.05047052  1.50506222  7.64184795]\n",
      " [ 3.06565867  1.50506222  7.64184795]\n",
      " [ 0.03826924  2.07773881  7.64184795]\n",
      " [ 0.0815894   1.49649622  8.25718808]\n",
      " [-2.96471763  1.50506222  7.64184795]\n",
      " [ 0.06267181  0.93238563  7.64184795]\n",
      " [ 0.01935164  1.51362822  7.02650781]]\n",
      "sigmax = \n",
      "[[ 0.02056537  1.60495871  7.64588411]\n",
      " [ 3.03575352  1.40545391  7.65043775]\n",
      " [ 0.00836408  2.17767711  7.64466497]\n",
      " [ 0.05168425  1.59621877  8.26433045]\n",
      " [-2.99462278  1.40647158  7.62525022]\n",
      " [ 0.03276665  1.03222544  7.64710264]\n",
      " [-0.01055351  1.61360193  7.02743386]]\n",
      "sqP = \n",
      "[[1.5726154034478947, 0.0, 0.0], [-0.0013977808929029284, 0.5380002005592256, 0.0], [0.009098356937767045, -0.0022440278625720797, 0.5436762811241043]]\n",
      "m = \n",
      "[0.02363512 1.49738679 7.818445  ]\n",
      "P = \n",
      "[[ 2.47306127e+00 -1.70869575e-03  1.09200046e-02]\n",
      " [-1.70869575e-03  8.53476211e-02 -3.65919541e-03]\n",
      " [ 1.09200046e-02 -3.65919541e-03  9.26988995e-02]]\n",
      "num = \n",
      "434\n",
      "SENSOR 9 3.59551880688 -0.808067235008\n",
      "\n",
      "num = \n",
      "435\n",
      "SENSOR 2 4.17678928397 -2.9234396319\n",
      "\n",
      "num = \n",
      "436\n",
      "SENSOR 3 1.07794432908 -2.21510076284\n",
      "\n",
      "num = \n",
      "437\n",
      "SENSOR 7 4.51763234841 -1.82762500095\n",
      "\n",
      "num = \n",
      "438\n",
      "ODOMETRY -0.0106424165584 0.0998962699375 -0.0193739099135\n",
      "\n",
      "sqP = \n",
      "[[1.5725969828511486, 0.0, 0.0], [-0.0010865439573139136, 0.29214113114609663, 0.0], [0.0069439307845948536, -0.012499611082308222, 0.304128987537033]]\n",
      "sigma = \n",
      "[[ 0.02363512  1.49738679  7.818445  ]\n",
      " [ 3.16882909  1.49738679  7.818445  ]\n",
      " [ 0.02146203  2.08166906  7.818445  ]\n",
      " [ 0.03752298  1.47238757  8.42670298]\n",
      " [-3.12155884  1.49738679  7.818445  ]\n",
      " [ 0.02580821  0.91310453  7.818445  ]\n",
      " [ 0.00974726  1.52238601  7.21018703]]\n",
      "sigmax = \n",
      "[[-5.53599043e-03  1.59744483e+00  7.81982404e+00]\n",
      " [ 3.13965798e+00  1.39733437e+00  7.81670563e+00]\n",
      " [-7.70907834e-03  2.18172985e+00  7.81960660e+00]\n",
      " [ 8.35187114e-03  1.57241681e+00  8.42947143e+00]\n",
      " [ 3.13245535e+00  1.39732444e+00  7.81742631e+00]\n",
      " [-3.36290251e-03  1.01315933e+00  7.82004147e+00]\n",
      " [-1.94238520e-02  1.62245355e+00  7.21017638e+00]]\n",
      "sqP = \n",
      "[[1.4319928304006975, 0.0, 0.0], [-0.08259821898114389, 0.5349670474998263, 0.0], [0.00034348010123447616, -0.00694062225970945, 0.5411753003928792]]\n",
      "m = \n",
      "[0.77435558 1.58291805 7.83751743]\n",
      "P = \n",
      "[[ 2.04581020e+00 -8.72329581e-02  2.19700500e-04]\n",
      " [-8.72329581e-02  9.18972081e-02 -1.76828593e-04]\n",
      " [ 2.19700500e-04 -1.76828593e-04  8.61527272e-02]]\n",
      "num = \n",
      "439\n",
      "SENSOR 9 3.54948085637 -0.797013500387\n",
      "\n",
      "num = \n",
      "440\n",
      "SENSOR 2 4.28131949654 -2.97537943488\n",
      "\n",
      "num = \n",
      "441\n",
      "SENSOR 3 1.24246440412 -2.3096004705\n",
      "\n",
      "num = \n",
      "442\n",
      "SENSOR 7 4.5411888941 -1.80078211601\n",
      "\n",
      "num = \n",
      "443\n",
      "ODOMETRY -0.0104250459457 0.100075632863 -0.0188431839508\n",
      "\n",
      "sqP = \n",
      "[[1.430318216552352, 0.0, 0.0], [-0.060988496874150526, 0.29694715251435305, 0.0], [0.00015360253259497355, -0.0005639407689275647, 0.293517266239341]]\n",
      "sigma = \n",
      "[[ 0.77435558  1.58291805  7.83751743]\n",
      " [ 3.63499202  1.58291805  7.83751743]\n",
      " [ 0.65237859  2.17681236  7.83751743]\n",
      " [ 0.77466279  1.58179017  8.42455196]\n",
      " [-2.08628085  1.58291805  7.83751743]\n",
      " [ 0.89633258  0.98902375  7.83751743]\n",
      " [ 0.77404838  1.58404593  7.2504829 ]]\n",
      "sigmax = \n",
      "[[ 0.74433926  1.65507042  7.9066065 ]\n",
      " [-2.67820962  1.49443808  7.7911433 ]\n",
      " [ 0.62236226  2.25683503  7.89731405]\n",
      " [ 0.74464646  1.65392131  8.4936632 ]\n",
      " [-2.11629718  1.53275136  7.75113128]\n",
      " [ 0.86631625  1.05223363  7.91487229]\n",
      " [ 0.74403205  1.65621953  7.3195498 ]]\n",
      "sqP = \n",
      "[[1.440140460165701, 0.0, 0.0], [0.04597437366518608, 0.5407526429657471, 0.0], [0.054581312443124835, -0.0009453398856435016, 0.5354722646540175]]\n",
      "m = \n",
      "[-0.03423339  1.61572283  7.94267649]\n",
      "P = \n",
      "[[2.0703988  0.0487416  0.05783077]\n",
      " [0.0487416  0.08891624 0.00212467]\n",
      " [0.05783077 0.00212467 0.08610079]]\n",
      "num = \n",
      "444\n",
      "SENSOR 9 3.48168308597 -0.705073049043\n",
      "\n",
      "num = \n",
      "445\n",
      "SENSOR 2 4.51016536991 -2.85015444599\n",
      "\n",
      "num = \n",
      "446\n",
      "SENSOR 3 1.17397867689 -2.26384820849\n",
      "\n",
      "num = \n",
      "447\n",
      "SENSOR 7 4.58368921745 -1.81442387473\n",
      "\n",
      "num = \n",
      "448\n",
      "ODOMETRY -0.0101771473921 0.100011050735 -0.0196083731561\n",
      "\n",
      "sqP = \n",
      "[[1.438888042237085, 0.0, 0.0], [0.03387448946107738, 0.296257928568624, 0.0], [0.04019129353634579, 0.0025761840174339553, 0.29065239525818587]]\n",
      "sigma = \n",
      "[[-0.03423339  1.61572283  7.94267649]\n",
      " [ 2.8435427   1.61572283  7.94267649]\n",
      " [ 0.03351559  2.20823869  7.94267649]\n",
      " [ 0.0461492   1.6208752   8.52398128]\n",
      " [-2.91200947  1.61572283  7.94267649]\n",
      " [-0.10198237  1.02320698  7.94267649]\n",
      " [-0.11461598  1.61057047  7.3613717 ]]\n",
      "sigmax = \n",
      "[[-6.35016180e-02  1.71569869e+00  7.93820876e+00]\n",
      " [ 2.81427447e+00  1.52037100e+00  7.97306005e+00]\n",
      " [ 4.24736095e-03  2.30828765e+00  7.94498709e+00]\n",
      " [ 1.68809691e-02  1.72088698e+00  8.52755564e+00]\n",
      " [-2.94127770e+00  1.51804093e+00  7.92091925e+00]\n",
      " [-1.31250597e-01  1.12265103e+00  7.93145093e+00]\n",
      " [-1.43884205e-01  1.70986476e+00  7.34889073e+00]]\n",
      "sqP = \n",
      "[[1.5077007724363762, 0.0, 0.0], [0.0072890044478859045, 0.5431748417723135, 0.0], [0.020371312226586, 0.0024876563355669392, 0.5353529728711778]]\n",
      "m = \n",
      "[-0.05918109  1.68845304  8.04636229]\n",
      "P = \n",
      "[[2.27282536e+00 8.15462839e-03 2.28640674e-02]\n",
      " [8.15462839e-03 8.78373723e-02 8.34164073e-05]\n",
      " [2.28640674e-02 8.34164073e-05 8.08955290e-02]]\n",
      "num = \n",
      "449\n",
      "SENSOR 9 3.53348326087 -0.784447831034\n",
      "\n",
      "num = \n",
      "450\n",
      "SENSOR 2 4.52117731575 -2.89659106551\n",
      "\n",
      "num = \n",
      "451\n",
      "SENSOR 3 1.39890586374 -2.14286560722\n",
      "\n",
      "num = \n",
      "452\n",
      "SENSOR 7 4.76172397197 -1.82219242799\n",
      "\n",
      "num = \n",
      "453\n",
      "ODOMETRY -0.0101785548489 0.100004838788 -0.0202689362584\n",
      "\n",
      "sqP = \n",
      "[[1.5075892558645279, 0.0, 0.0], [0.005409051807983267, 0.2963243400376246, 0.0], [0.015165979251270693, 4.666642693588595e-06, 0.284016763616788]]\n",
      "sigma = \n",
      "[[-0.05918109  1.68845304  8.04636229]\n",
      " [ 2.95599742  1.68845304  8.04636229]\n",
      " [-0.04836299  2.28110172  8.04636229]\n",
      " [-0.02884913  1.68846237  8.61439581]\n",
      " [-3.0743596   1.68845304  8.04636229]\n",
      " [-0.06999919  1.09580436  8.04636229]\n",
      " [-0.08951305  1.6884437   7.47832876]]\n",
      "sigmax = \n",
      "[[-0.08896661  1.78822363  8.03943126]\n",
      " [ 2.9262119   1.59035243  8.06581686]\n",
      " [-0.07814851  2.38094145  8.04051097]\n",
      " [-0.05863465  1.78839727  8.61049375]\n",
      " [-3.10414512  1.58860473  8.04065916]\n",
      " [-0.09978471  1.19549413  8.03835235]\n",
      " [-0.11929857  1.7879582   7.46837514]]\n",
      "sqP = \n",
      "[[1.5726044096231804, 0.0, 0.0], [0.0014392707067166384, 0.5433538027033601, 0.0], [0.008784869274402175, -0.00061292785301102, 0.5305894985329488]]\n",
      "m = \n",
      "[-0.08704885  1.61845048  8.17789812]\n",
      "P = \n",
      "[[ 2.47302679e+00  1.72041276e-03  1.04092810e-02]\n",
      " [ 1.72041276e-03  8.87406277e-02 -7.96853274e-05]\n",
      " [ 1.04092810e-02 -7.96853274e-05  7.58957830e-02]]\n",
      "num = \n",
      "454\n",
      "SENSOR 9 3.44733618805 -0.725368623109\n",
      "\n",
      "num = \n",
      "455\n",
      "SENSOR 2 4.69146013992 -2.87115134275\n",
      "\n",
      "num = \n",
      "456\n",
      "SENSOR 3 1.50765435383 -2.22245197353\n",
      "\n",
      "num = \n",
      "457\n",
      "SENSOR 7 4.48642435312 -1.84876318884\n",
      "\n",
      "num = \n",
      "458\n",
      "ODOMETRY -0.0091227397028 0.0999924972858 -0.0181685651638\n",
      "\n",
      "sqP = \n",
      "[[1.572586020076137, 0.0, 0.0], [0.0010940023254060905, 0.29789164280641434, 0.0], [0.006619212460802461, -0.00029180664617414363, 0.27541220711986625]]\n",
      "sigma = \n",
      "[[-0.08704885  1.61845048  8.17789812]\n",
      " [ 3.05812319  1.61845048  8.17789812]\n",
      " [-0.08486085  2.21423376  8.17789812]\n",
      " [-0.07381043  1.61786686  8.72872253]\n",
      " [-3.23222089  1.61845048  8.17789812]\n",
      " [-0.08923686  1.02266719  8.17789812]\n",
      " [-0.10028728  1.61903409  7.6270737 ]]\n",
      "sigmax = \n",
      "[[-0.11749634  1.71798301  8.16819022]\n",
      " [ 3.0276757   1.51888384  8.18724969]\n",
      " [-0.11530834  2.31378729  8.16840802]\n",
      " [-0.10425792  1.71751918  8.7203331 ]\n",
      " [ 3.02051693  1.51895333  8.18796222]\n",
      " [-0.11968435  1.12217824  8.16797246]\n",
      " [-0.13073477  1.71842938  7.61604904]]\n",
      "sqP = \n",
      "[[1.4319912845478444, 0.0, 0.0], [-0.0816572677570271, 0.5380591733105213, 0.0], [0.009262496289077582, 2.432966792028819e-06, 0.5255474620645623]]\n",
      "m = \n",
      "[0.65834554 1.73930633 8.24298859]\n",
      "P = \n",
      "[[ 2.04587734 -0.08628531  0.00925832]\n",
      " [-0.08628531  0.09456452  0.00209119]\n",
      " [ 0.00925832  0.00209119  0.07353352]]\n",
      "num = \n",
      "459\n",
      "SENSOR 9 3.22534891044 -0.724699872244\n",
      "\n",
      "num = \n",
      "460\n",
      "SENSOR 2 4.82403976411 -2.9345124872\n",
      "\n",
      "num = \n",
      "461\n",
      "SENSOR 3 1.40944143312 -2.41378091735\n",
      "\n",
      "num = \n",
      "462\n",
      "SENSOR 7 4.61353587116 -1.78898785581\n",
      "\n",
      "num = \n",
      "463\n",
      "ODOMETRY -0.00990767375964 0.0999963798923 -0.0204463659192\n",
      "\n",
      "sqP = \n",
      "[[1.4303416878693171, 0.0, 0.0], [-0.060324962074393675, 0.3015384200628323, 0.0], [0.006472800710594657, 0.008229985652217608, 0.2709684318379876]]\n",
      "sigma = \n",
      "[[ 0.65834554  1.73930633  8.24298859]\n",
      " [ 3.51902892  1.73930633  8.24298859]\n",
      " [ 0.53769562  2.34238317  8.24298859]\n",
      " [ 0.67129114  1.7557663   8.78492545]\n",
      " [-2.20233783  1.73930633  8.24298859]\n",
      " [ 0.77899547  1.13622949  8.24298859]\n",
      " [ 0.64539994  1.72284636  7.70105173]]\n",
      "sigmax = \n",
      "[[ 0.63105424  1.81895575  8.3034408 ]\n",
      " [-2.79144769  1.64601974  8.20698703]\n",
      " [ 0.51040431  2.42872946  8.29341496]\n",
      " [ 0.64399984  1.83462647  8.84640368]\n",
      " [-2.22962914  1.67953801  8.16282472]\n",
      " [ 0.75170416  1.20802403  8.31258775]\n",
      " [ 0.61810864  1.80327167  7.76046779]]\n",
      "sqP = \n",
      "[[1.4401247616086204, 0.0, 0.0], [0.0518696092539639, 0.5432640207976007, 0.0], [0.048767488381735176, 0.0027715658445357535, 0.5235364273507965]]\n",
      "m = \n",
      "[-0.1409082   1.86122429  8.33476999]\n",
      "P = \n",
      "[[2.0703468  0.05511701 0.05141837]\n",
      " [0.05511701 0.09239781 0.00438214]\n",
      " [0.05141837 0.00438214 0.07305373]]\n",
      "num = \n",
      "464\n",
      "SENSOR 9 3.2978264557 -0.848937354757\n",
      "\n",
      "num = \n",
      "465\n",
      "SENSOR 2 4.79210487087 -2.88027569823\n",
      "\n",
      "num = \n",
      "466\n",
      "SENSOR 3 1.57460267827 -2.35009049783\n",
      "\n",
      "num = \n",
      "467\n",
      "SENSOR 7 4.86239210585 -1.68728512464\n",
      "\n",
      "num = \n",
      "468\n",
      "ODOMETRY -0.0105209157752 0.100056520755 -0.0199923529952\n",
      "\n",
      "sqP = \n",
      "[[1.438869972447588, 0.0, 0.0], [0.03830575995722656, 0.3015468036724356, 0.0], [0.035735241641304295, 0.00999273651061487, 0.2677253634351728]]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "sigma = \n",
      "[[-0.1409082   1.86122429  8.33476999]\n",
      " [ 2.73683174  1.86122429  8.33476999]\n",
      " [-0.06429668  2.46431789  8.33476999]\n",
      " [-0.06943772  1.88120976  8.87022072]\n",
      " [-3.01864815  1.86122429  8.33476999]\n",
      " [-0.21751972  1.25813068  8.33476999]\n",
      " [-0.21237869  1.84123881  7.79931926]]\n",
      "sigmax = \n",
      "[[-0.17126224  1.96008559  8.31974606]\n",
      " [ 2.7064777   1.76970261  8.3750572 ]\n",
      " [-0.09465072  2.56403909  8.32735663]\n",
      " [-0.09979176  1.98089153  8.86229479]\n",
      " [-3.04900219  1.76186607  8.32349077]\n",
      " [-0.24787376  1.35555212  8.31222362]\n",
      " [-0.24273273  1.93877488  7.77727403]]\n",
      "sqP = \n",
      "[[1.5076773979709273, 0.0, 0.0], [0.009795335534588848, 0.5462250725075731, 0.0], [0.01882871919759561, 0.005064156202610765, 0.5230132837548727]]\n",
      "m = \n",
      "[-0.16940254  1.78454189  8.44254175]\n",
      "P = \n",
      "[[2.27275845e+00 1.09454447e-02 2.10150660e-02]\n",
      " [1.09454447e-02 9.11022953e-02 3.93633622e-05]\n",
      " [2.10150660e-02 3.93633622e-05 6.95967781e-02]]\n",
      "num = \n",
      "469\n",
      "SENSOR 9 3.11237333983 -0.828608726065\n",
      "\n",
      "num = \n",
      "470\n",
      "SENSOR 2 5.1348537585 -2.83576534622\n",
      "\n",
      "num = \n",
      "471\n",
      "SENSOR 3 1.7099726366 -2.2858612058\n",
      "\n",
      "num = \n",
      "472\n",
      "SENSOR 7 4.90049538427 -1.78150085169\n",
      "\n",
      "num = \n",
      "473\n",
      "ODOMETRY -0.010155344285 0.100089786643 -0.0198085659559\n",
      "\n",
      "sqP = \n",
      "[[1.5075670624872342, 0.0, 0.0], [0.0072603368582143165, 0.3017442341407329, 0.0], [0.013939722159261664, -0.0002049540948623433, 0.2634433909073669]]\n",
      "sigma = \n",
      "[[-0.16940254  1.78454189  8.44254175]\n",
      " [ 2.84573159  1.78454189  8.44254175]\n",
      " [-0.15488187  2.38803036  8.44254175]\n",
      " [-0.1415231   1.78413198  8.96942853]\n",
      " [-3.18453666  1.78454189  8.44254175]\n",
      " [-0.18392321  1.18105342  8.44254175]\n",
      " [-0.19728198  1.7849518   7.91565497]]\n",
      "sigmax = \n",
      "[[-0.19991581  1.88298324  8.42463621]\n",
      " [ 2.81521832  1.6891449   8.47271991]\n",
      " [-0.18539513  2.48672132  8.42606748]\n",
      " [-0.17203636  1.88303421  8.95427408]\n",
      " [ 3.06813537  1.68462834  8.44788872]\n",
      " [-0.21443648  1.2792244   8.42320871]\n",
      " [-0.22779525  1.88285576  7.89501225]]\n",
      "sqP = \n",
      "[[1.4334558916877471, 0.0, 0.0], [-0.07909553330096038, 0.5404462374438723, 0.0], [0.01696085538992685, 0.0004900881524613313, 0.519727682764756]]\n",
      "m = \n",
      "[0.58412947 1.88798138 8.63357854]\n",
      "P = \n",
      "[[ 2.05023986e+00 -8.37906522e-02  1.74327382e-02]\n",
      " [-8.37906522e-02  9.65758952e-02  7.82513593e-04]\n",
      " [ 1.74327382e-02  7.82513593e-04  6.87759446e-02]]\n",
      "num = \n",
      "474\n",
      "SENSOR 9 2.99996691085 -0.765112595878\n",
      "\n",
      "num = \n",
      "475\n",
      "SENSOR 3 1.6542766528 -2.40104297088\n",
      "\n",
      "num = \n",
      "476\n",
      "SENSOR 7 4.68837972288 -1.72827448368\n",
      "\n",
      "num = \n",
      "477\n",
      "ODOMETRY -0.00967291193757 0.100124175068 -0.0205587676625\n",
      "\n",
      "sqP = \n",
      "[[1.4318658674580698, 0.0, 0.0], [-0.058518506604612434, 0.3052072731791708, 0.0], [0.012174840266833138, 0.004898202616217214, 0.26192312895389375]]\n",
      "sigma = \n",
      "[[ 0.58412947  1.88798138  8.63357854]\n",
      " [ 3.44786121  1.88798138  8.63357854]\n",
      " [ 0.46709246  2.49839593  8.63357854]\n",
      " [ 0.60847915  1.89777779  9.1574248 ]\n",
      " [-2.27960226  1.88798138  8.63357854]\n",
      " [ 0.70116648  1.27756684  8.63357854]\n",
      " [ 0.55977979  1.87818498  8.10973228]]\n",
      "sigmax = \n",
      "[[ 0.55416556  1.97203176  8.68792465]\n",
      " [-2.86528801  1.79224771  8.60437186]\n",
      " [ 0.43712855  2.58821731  8.6777383 ]\n",
      " [ 0.57851524  1.98048007  9.21380119]\n",
      " [-2.30956617  1.82206191  8.55826195]\n",
      " [ 0.67120257  1.35469623  8.69736743]\n",
      " [ 0.52981588  1.96353362  8.16201588]]\n",
      "sqP = \n",
      "[[1.4399392834722418, 0.0, 0.0], [0.05558838473382832, 0.5451298929946992, 0.0], [0.044868531706049694, 0.0005656729623968998, 0.5190472652527851]]\n",
      "m = \n",
      "[-0.22680421  2.02028009  8.60661775]\n",
      "P = \n",
      "[[2.07042403 0.06180323 0.05139103]\n",
      " [0.06180323 0.10550343 0.02155377]\n",
      " [0.05139103 0.02155377 0.09618274]]\n",
      "num = \n",
      "478\n",
      "SENSOR 9 2.99807347078 -0.696237174014\n",
      "\n",
      "num = \n",
      "479\n",
      "SENSOR 3 1.7267487227 -2.3662440395\n",
      "\n",
      "num = \n",
      "480\n",
      "SENSOR 7 4.7350984348 -1.75139975295\n",
      "\n",
      "num = \n",
      "481\n",
      "ODOMETRY -0.0101662059319 0.100228934457 -0.0199252685348\n",
      "\n",
      "sqP = \n",
      "[[1.4388968111596103, 0.0, 0.0], [0.042951813686972706, 0.3219605189408196, 0.0], [0.035715577024837415, 0.062180674025287014, 0.30172952068291936]]\n",
      "sigma = \n",
      "[[-0.22680421  2.02028009  8.60661775]\n",
      " [ 2.65098942  2.02028009  8.60661775]\n",
      " [-0.14090058  2.66420113  8.60661775]\n",
      " [-0.15537305  2.14464144  9.2100768 ]\n",
      " [-3.10459783  2.02028009  8.60661775]\n",
      " [-0.31270783  1.37635905  8.60661775]\n",
      " [-0.29823536  1.89591874  8.00315871]]\n",
      "sigmax = \n",
      "[[-0.25703588  2.11761774  8.58316074]\n",
      " [ 2.62075774  1.93242612  8.6546441 ]\n",
      " [-0.17113226  2.76319242  8.59159861]\n",
      " [-0.18560473  2.24340501  9.19362663]\n",
      " [-3.13482951  1.92019328  8.60388251]\n",
      " [-0.34293951  1.47132521  8.57489586]\n",
      " [-0.32846704  1.99133404  7.97281448]]\n",
      "sqP = \n",
      "[[1.5078277401303215, 0.0, 0.0], [0.013611085303299983, 0.5612046473108081, 0.0], [0.019458475714929112, 0.033331078210508465, 0.5405880704793017]]\n",
      "m = \n",
      "[-0.2559367   2.06161727  8.63222304]\n",
      "P = \n",
      "[[2.27318657 0.01539974 0.02256701]\n",
      " [0.01539974 0.10437483 0.01862925]\n",
      " [0.02256701 0.01862925 0.09764331]]\n",
      "num = \n",
      "482\n",
      "SENSOR 9 3.15312197773 -0.769591095276\n",
      "\n",
      "num = \n",
      "483\n",
      "SENSOR 3 1.73627144755 -2.31623144182\n",
      "\n",
      "num = \n",
      "484\n",
      "SENSOR 7 5.06411728014 -1.71705818396\n",
      "\n",
      "num = \n",
      "485\n",
      "ODOMETRY -0.0107211355799 0.0999927335192 -0.020501983192\n",
      "\n",
      "sqP = \n",
      "[[1.5077090458564895, 0.0, 0.0], [0.010214001528259494, 0.32290943680752765, 0.0], [0.014967747573741407, 0.05721841840610942, 0.3068311046191775]]\n",
      "sigma = \n",
      "[[-0.2559367   2.06161727  8.63222304]\n",
      " [ 2.75948139  2.06161727  8.63222304]\n",
      " [-0.2355087   2.70743614  8.63222304]\n",
      " [-0.2260012   2.1760541   9.24588525]\n",
      " [-3.27135479  2.06161727  8.63222304]\n",
      " [-0.2763647   1.41579839  8.63222304]\n",
      " [-0.28587219  1.94718043  8.01856083]]\n",
      "sigmax = \n",
      "[[-0.28602817  2.15831845  8.60586549]\n",
      " [ 2.72938992  1.96900164  8.67053995]\n",
      " [-0.26560017  2.80465554  8.60784626]\n",
      " [-0.25609268  2.27350087  9.22243387]\n",
      " [ 2.98173904  1.96236797  8.64620219]\n",
      " [-0.30645618  1.51194101  8.60389571]\n",
      " [-0.31596367  2.04304938  7.98932072]]\n",
      "sqP = \n",
      "[[1.4334780045452762, 0.0, 0.0], [-0.07631749901021256, 0.5557861379791285, 0.0], [0.024531531390143597, 0.03250573036290528, 0.5421548422222361]]\n",
      "m = \n",
      "[0.53192619 1.91559303 8.72822179]\n",
      "P = \n",
      "[[ 2.05043404 -0.08126426  0.0246611 ]\n",
      " [-0.08126426  0.10952013  0.01540724]\n",
      " [ 0.0246611   0.01540724  0.09715849]]\n",
      "num = \n",
      "486\n",
      "SENSOR 9 2.86278392437 -0.743762595525\n",
      "\n",
      "num = \n",
      "487\n",
      "SENSOR 3 1.84695799822 -2.43783547802\n",
      "\n",
      "num = \n",
      "488\n",
      "SENSOR 7 5.07231532054 -1.68808003478\n",
      "\n",
      "num = \n",
      "489\n",
      "ODOMETRY -0.00979459746259 0.100025076699 -0.0197251786846\n",
      "\n",
      "sqP = \n",
      "[[1.4319336705353394, 0.0, 0.0], [-0.05675141407281033, 0.32603589761351864, 0.0], [0.017222235096391363, 0.05025406762846905, 0.30714232619687304]]\n",
      "sigma = \n",
      "[[ 0.53192619  1.91559303  8.72822179]\n",
      " [ 3.39579353  1.91559303  8.72822179]\n",
      " [ 0.41842336  2.56766482  8.72822179]\n",
      " [ 0.56637066  2.01610116  9.34250644]\n",
      " [-2.33194115  1.91559303  8.72822179]\n",
      " [ 0.64542902  1.26352123  8.72822179]\n",
      " [ 0.49748172  1.81508489  8.11393714]]\n",
      "sigmax = \n",
      "[[ 0.50070307  2.0023087   8.77801073]\n",
      " [-2.9186149   1.81854958  8.70411542]\n",
      " [ 0.38720024  2.65946158  8.767869  ]\n",
      " [ 0.53514754  2.10105079  9.39525213]\n",
      " [-2.36316427  1.84585073  8.65656595]\n",
      " [ 0.6142059   1.34403987  8.78751171]\n",
      " [ 0.4662586   1.90346375  8.16071026]]\n",
      "sqP = \n",
      "[[1.4399135359071715, 0.0, 0.0], [0.05779377318477211, 0.55920046959458, 0.0], [0.04264635793561793, 0.025380294534335823, 0.5429021977467211]]\n",
      "m = \n",
      "[-0.26998665  2.03172753  8.85194485]\n",
      "P = \n",
      "[[2.07019698 0.06243898 0.0477545 ]\n",
      " [0.06243898 0.10893611 0.02514648]\n",
      " [0.0477545  0.02514648 0.10011219]]\n",
      "num = \n",
      "490\n",
      "SENSOR 9 2.82075829413 -0.688148179119\n",
      "\n",
      "num = \n",
      "491\n",
      "SENSOR 3 2.02116189813 -2.39856289262\n",
      "\n",
      "num = \n",
      "492\n",
      "SENSOR 7 4.91405340171 -1.71247547809\n",
      "\n",
      "num = \n",
      "493\n",
      "ODOMETRY -0.00880991332931 0.0997863392491 -0.0197458769482\n",
      "\n",
      "sqP = \n",
      "[[1.4388179104307488, 0.0, 0.0], [0.04339602581300737, 0.3271893875179337, 0.0], [0.03319009164696958, 0.07245395204043523, 0.3062042291609134]]\n",
      "sigma = \n",
      "[[-0.26998665  2.03172753  8.85194485]\n",
      " [ 2.60764917  2.03172753  8.85194485]\n",
      " [-0.1831946   2.6861063   8.85194485]\n",
      " [-0.20360647  2.17663543  9.46435331]\n",
      " [-3.14762247  2.03172753  8.85194485]\n",
      " [-0.3567787   1.37734875  8.85194485]\n",
      " [-0.33636683  1.88681963  8.2395364 ]]\n",
      "sigmax = \n",
      "[[-0.29950643  2.12786322  8.82432339]\n",
      " [ 2.57812939  1.94612799  8.90369169]\n",
      " [-0.21271438  2.78427445  8.8327607 ]\n",
      " [-0.23312624  2.27439157  9.4431695 ]\n",
      " [ 3.10604306  1.93171498  8.85352763]\n",
      " [-0.38629848  1.47072827  8.81609402]\n",
      " [-0.36588661  1.98091142  8.20559894]]\n",
      "sqP = \n",
      "[[1.4390809306047176, 0.0, 0.0], [-0.0659955712835565, 0.5617459289283767, 0.0], [0.028309273858874908, 0.04283540159515256, 0.5420457567304933]]\n",
      "m = \n",
      "[0.48172751 2.15774647 8.91780425]\n",
      "P = \n",
      "[[ 2.06733476 -0.07044485  0.02874905]\n",
      " [-0.07044485  0.11028698  0.02057711]\n",
      " [ 0.02874905  0.02057711  0.09843152]]\n",
      "num = \n",
      "494\n",
      "SENSOR 9 2.83728718149 -0.703540952524\n",
      "\n",
      "num = \n",
      "495\n",
      "SENSOR 3 1.93000650123 -2.36970627286\n",
      "\n",
      "num = \n",
      "496\n",
      "SENSOR 7 4.79347993043 -1.6329273271\n",
      "\n",
      "num = \n",
      "497\n",
      "ODOMETRY -0.0100580444163 0.10021748682 -0.0200095441708\n",
      "\n",
      "sqP = \n",
      "[[1.437822922572896, 0.0, 0.0], [-0.04899410988115665, 0.32846089488467295, 0.0], [0.019994847471960644, 0.06562955440452953, 0.30614455429711157]]\n",
      "sigma = \n",
      "[[ 0.48172751  2.15774647  8.91780425]\n",
      " [ 3.35737336  2.15774647  8.91780425]\n",
      " [ 0.38373929  2.81466826  8.91780425]\n",
      " [ 0.52171721  2.28900558  9.53009336]\n",
      " [-2.39391833  2.15774647  8.91780425]\n",
      " [ 0.57971573  1.50082468  8.91780425]\n",
      " [ 0.44173782  2.02648736  8.30551514]]\n",
      "sigmax = \n",
      "[[ 0.45317172  2.24658057  8.96325549]\n",
      " [-2.95436774  2.06008978  8.89729852]\n",
      " [ 0.3551835   2.90752279  8.95434668]\n",
      " [ 0.49316142  2.37595156  9.57905976]\n",
      " [-2.42247412  2.08398103  8.85060325]\n",
      " [ 0.55115994  1.58478608  8.97172823]\n",
      " [ 0.41318203  2.11706754  8.34737854]]\n",
      "sqP = \n",
      "[[1.4391083934725404, 0.0, 0.0], [0.061569169379673584, 0.5616825676501581, 0.0], [0.039866501772589207, 0.033621919086096984, 0.5419804821450546]]\n",
      "m = \n",
      "[-0.33536843  2.21276192  8.87747492]\n",
      "P = \n",
      "[[2.06765946 0.0658623  0.04418139]\n",
      " [0.0658623  0.11094487 0.02274986]\n",
      " [0.04418139 0.02274986 0.0965521 ]]\n",
      "num = \n",
      "498\n",
      "SENSOR 9 2.51641560778 -0.756300137634\n",
      "\n",
      "num = \n",
      "499\n",
      "SENSOR 3 2.09607292645 -2.38526777678\n",
      "\n",
      "num = \n",
      "500\n",
      "SENSOR 7 5.06835478059 -1.72475485601\n",
      "\n",
      "num = \n",
      "501\n",
      "ODOMETRY -0.0104281508158 0.10000140247 -0.0199377152457\n",
      "\n",
      "sqP = \n",
      "[[1.437935834770966, 0.0, 0.0], [0.04580336480919663, 0.3299195751257076, 0.0], [0.03072556532910229, 0.06469008272113863, 0.3023627542246576]]\n",
      "sigma = \n",
      "[[-0.33536843  2.21276192  8.87747492]\n",
      " [ 2.54050324  2.21276192  8.87747492]\n",
      " [-0.2437617   2.87260107  8.87747492]\n",
      " [-0.2739173   2.34214209  9.48220043]\n",
      " [-3.2112401   2.21276192  8.87747492]\n",
      " [-0.42697516  1.55292277  8.87747492]\n",
      " [-0.39681956  2.08338176  8.27274941]]\n",
      "sigmax = \n",
      "[[-0.36543602  2.30705968  8.84354148]\n",
      " [ 2.51043565  2.13068479  8.93498048]\n",
      " [-0.27382929  2.96960763  8.85230999]\n",
      " [-0.30398489  2.43834579  9.4541221 ]\n",
      " [ 3.04187762  2.11286261  8.88545435]\n",
      " [-0.45704275  1.64372095  8.83505753]\n",
      " [-0.42688715  2.17541759  8.23308896]]\n",
      "sqP = \n",
      "[[1.439181970474107, 0.0, 0.0], [-0.06421680072778159, 0.5625957809294237, 0.0], [0.03277321843028844, 0.037936756385973686, 0.5400676734887964]]\n",
      "m = \n",
      "[0.42194884 2.35230845 9.04155692]\n",
      "P = \n",
      "[[ 2.06761649 -0.06860896  0.03391493]\n",
      " [-0.06860896  0.1100758   0.01598323]\n",
      " [ 0.03391493  0.01598323  0.09588995]]\n",
      "num = \n",
      "502\n",
      "SENSOR 9 2.45948289837 -0.76021264418\n",
      "\n",
      "num = \n",
      "503\n",
      "SENSOR 3 2.06522117309 -2.33318420312\n",
      "\n",
      "num = \n",
      "504\n",
      "SENSOR 7 4.84613918824 -1.61007685337\n",
      "\n",
      "num = \n",
      "505\n",
      "ODOMETRY -0.0097623943034 0.0999813928244 -0.0206369980195\n",
      "\n",
      "sqP = \n",
      "[[1.437920891232243, 0.0, 0.0], [-0.047714002420664606, 0.3283278480818069, 0.0], [0.023586086616444842, 0.05210834209786366, 0.3043326548654499]]\n",
      "sigma = \n",
      "[[ 0.42194884  2.35230845  9.04155692]\n",
      " [ 3.29779062  2.35230845  9.04155692]\n",
      " [ 0.32652083  3.00896415  9.04155692]\n",
      " [ 0.46912101  2.45652514  9.65022223]\n",
      " [-2.45389295  2.35230845  9.04155692]\n",
      " [ 0.51737684  1.69565275  9.04155692]\n",
      " [ 0.37477666  2.24809177  8.43289161]]\n",
      "sigmax = \n",
      "[[ 0.39158297  2.4439611   9.08155784]\n",
      " [-3.01576056  2.25336763  9.02703131]\n",
      " [ 0.29615496  3.10401121  9.07264288]\n",
      " [ 0.43875514  2.5461896   9.6945005 ]\n",
      " [-2.48425881  2.27437881  8.97888913]\n",
      " [ 0.48701097  1.78307698  9.0901088 ]\n",
      " [ 0.3444108   2.34152869  8.46852618]]\n",
      "sqP = \n",
      "[[1.439110710157178, 0.0, 0.0], [0.06401634655740478, 0.5600632657140406, 0.0], [0.03619696781533664, 0.02590478087733656, 0.5415505837604447]]\n",
      "m = \n",
      "[-0.38607475  2.4858591   9.02584981]\n",
      "P = \n",
      "[[2.06762944 0.06877031 0.04002527]\n",
      " [0.06877031 0.11199878 0.01949184]\n",
      " [0.04002527 0.01949184 0.09368501]]\n",
      "num = \n",
      "506\n",
      "SENSOR 9 2.57479230149 -0.632819077978\n",
      "\n",
      "num = \n",
      "507\n",
      "SENSOR 3 2.177346285 -2.42626146831\n",
      "\n",
      "num = \n",
      "508\n",
      "SENSOR 7 5.05551333216 -1.69863156486\n",
      "\n",
      "num = \n",
      "509\n",
      "ODOMETRY -0.00975220094369 0.100042185698 -0.0200047749536\n",
      "\n",
      "sqP = \n",
      "[[1.437925394417412, 0.0, 0.0], [0.047826060906484294, 0.3312271915941669, 0.0], [0.027835428705179383, 0.05482817744592506, 0.29984007313652655]]\n",
      "sigma = \n",
      "[[-0.38607475  2.4858591   9.02584981]\n",
      " [ 2.48977604  2.4858591   9.02584981]\n",
      " [-0.29042263  3.14831349  9.02584981]\n",
      " [-0.33040389  2.59551546  9.62552996]\n",
      " [-3.26192554  2.4858591   9.02584981]\n",
      " [-0.48172687  1.82340472  9.02584981]\n",
      " [-0.44174561  2.37620275  8.42616966]]\n",
      "sigmax = \n",
      "[[-0.41647414  2.57810934  8.98729891]\n",
      " [ 2.45937665  2.40697147  9.08727473]\n",
      " [-0.32082202  3.24382389  8.99628561]\n",
      " [-0.36080329  2.68976784  9.59217178]\n",
      " [ 2.99086038  2.3867226   9.03882026]\n",
      " [-0.51212626  1.91155141  8.97866465]\n",
      " [-0.472145    2.46616502  8.38254549]]\n",
      "sqP = \n",
      "[[1.4391899294631505, 0.0, 0.0], [-0.06243006337705415, 0.562436765670614, 0.0], [0.03584796063821412, 0.03218528652514159, 0.5388585428768656]]\n",
      "m = \n",
      "[0.38752321 2.44199677 9.12298411]\n",
      "P = \n",
      "[[ 2.0676829  -0.06685805  0.03747404]\n",
      " [-0.06685805  0.1104463   0.01182548]\n",
      " [ 0.03747404  0.01182548  0.09357571]]\n",
      "num = \n",
      "510\n",
      "SENSOR 9 2.5074325001 -0.609410447289\n",
      "\n",
      "num = \n",
      "511\n",
      "SENSOR 3 2.3010604256 -2.35179262067\n",
      "\n",
      "num = \n",
      "512\n",
      "SENSOR 7 4.90749675187 -1.70419293696\n",
      "\n",
      "num = \n",
      "513\n",
      "ODOMETRY -0.00927676045743 0.100208064048 -0.0191796860605\n",
      "\n",
      "sqP = \n",
      "[[1.4379439822559537, 0.0, 0.0], [-0.04649558479215177, 0.32906603906111614, 0.0], [0.026060845751862745, 0.039618774709278416, 0.30220340287297814]]\n",
      "sigma = \n",
      "[[ 0.38752321  2.44199677  9.12298411]\n",
      " [ 3.26341117  2.44199677  9.12298411]\n",
      " [ 0.29453204  3.10012885  9.12298411]\n",
      " [ 0.4396449   2.52123432  9.72739091]\n",
      " [-2.48836476  2.44199677  9.12298411]\n",
      " [ 0.48051438  1.7838647   9.12298411]\n",
      " [ 0.33540152  2.36275922  8.5185773 ]]\n",
      "sigmax = \n",
      "[[ 0.35776623  2.5349849   9.15988463]\n",
      " [-3.04953111  2.34258214  9.1117962 ]\n",
      " [ 0.26477506  3.19614169  9.15109058]\n",
      " [ 0.40988792  2.61217372  9.76908582]\n",
      " [-2.51812173  2.3619615   9.06296083]\n",
      " [ 0.4507574   1.87302458  9.16835981]\n",
      " [ 0.30564454  2.45754351  8.5505832 ]]\n",
      "sqP = \n",
      "[[1.4391113847580441, 0.0, 0.0], [0.06517530687628129, 0.5594036725948573, 0.0], [0.03417806631828164, 0.018842350175798345, 0.5407634679605645]]\n",
      "m = \n",
      "[-0.42218691  2.52133561  9.17041525]\n",
      "P = \n",
      "[[2.06763904 0.07031862 0.03790178]\n",
      " [0.07031862 0.11287864 0.01829657]\n",
      " [0.03790178 0.01829657 0.09226361]]\n",
      "num = \n",
      "514\n",
      "SENSOR 9 2.41626913018 -0.724594121384\n",
      "\n",
      "num = \n",
      "515\n",
      "SENSOR 3 2.52272282794 -2.4239408799\n",
      "\n",
      "num = \n",
      "516\n",
      "SENSOR 7 4.70640264651 -1.63618162589\n",
      "\n",
      "num = \n",
      "517\n",
      "ODOMETRY -0.0099067860188 0.0998711486494 -0.0194012350076\n",
      "\n",
      "sqP = \n",
      "[[1.437928732568257, 0.0, 0.0], [0.04890271261420835, 0.3323960895654902, 0.0], [0.026358592921776974, 0.05116655171917405, 0.2982462320871325]]\n",
      "sigma = \n",
      "[[-0.42218691  2.52133561  9.17041525]\n",
      " [ 2.45367055  2.52133561  9.17041525]\n",
      " [-0.32438149  3.18612779  9.17041525]\n",
      " [-0.36946973  2.62366871  9.76690771]\n",
      " [-3.29804438  2.52133561  9.17041525]\n",
      " [-0.51999234  1.85654343  9.17041525]\n",
      " [-0.4749041   2.4190025   8.57392278]]\n",
      "sigmax = \n",
      "[[-0.45064336  2.61236006  9.12850816]\n",
      " [ 2.42521411  2.44451172  9.23475611]\n",
      " [-0.35283793  3.28080943  9.13759694]\n",
      " [-0.39792617  2.71677491  9.72985517]\n",
      " [ 2.95668448  2.42250055  9.18694666]\n",
      " [-0.54844878  1.94304066  9.11981994]\n",
      " [-0.50336055  2.50769231  8.52727758]]\n",
      "sqP = \n",
      "[[1.4391979966362076, 0.0, 0.0], [-0.061313069263571794, 0.5628855620099493, 0.0], [0.03822075050694462, 0.029943566721938552, 0.5380157001199907]]\n",
      "m = \n",
      "[0.32681487 2.67881659 9.20343307]\n",
      "P = \n",
      "[[ 2.06765301 -0.06563224  0.03976639]\n",
      " [-0.06563224  0.11227341  0.01343758]\n",
      " [ 0.03976639  0.01343758  0.09263171]]\n",
      "num = \n",
      "518\n",
      "SENSOR 9 2.24529394017 -0.685502521964\n",
      "\n",
      "num = \n",
      "519\n",
      "SENSOR 3 2.31844097149 -2.42828855555\n",
      "\n",
      "num = \n",
      "520\n",
      "SENSOR 7 4.95528000197 -1.57288604154\n",
      "\n",
      "num = \n",
      "521\n",
      "ODOMETRY -0.00983384980744 0.0999224766995 -0.0204305528243\n",
      "\n",
      "sqP = \n",
      "[[1.4379335889039195, 0.0, 0.0], [-0.04564344589050446, 0.3319489262127845, 0.0], [0.027655234194158062, 0.04428350233318589, 0.29984306967938307]]\n",
      "sigma = \n",
      "[[ 0.32681487  2.67881659  9.20343307]\n",
      " [ 3.20268205  2.67881659  9.20343307]\n",
      " [ 0.23552798  3.34271445  9.20343307]\n",
      " [ 0.38212534  2.7673836   9.80311921]\n",
      " [-2.5490523   2.67881659  9.20343307]\n",
      " [ 0.41810176  2.01491874  9.20343307]\n",
      " [ 0.2715044   2.59024959  8.60374693]]\n",
      "sigmax = \n",
      "[[ 0.29750685  2.77371451  9.23455593]\n",
      " [-3.10981128  2.57907623  9.19832364]\n",
      " [ 0.20621996  3.44005439  9.22577543]\n",
      " [ 0.35281732  2.86041585  9.83944065]\n",
      " [-2.57836033  2.59542254  9.14848148]\n",
      " [ 0.38879374  2.10658436  9.24307725]\n",
      " [ 0.24219638  2.68672292  8.62957603]]\n",
      "sqP = \n",
      "[[1.4391148211505687, 0.0, 0.0], [0.06688746175551495, 0.5613099919080509, 0.0], [0.029805946228990028, 0.020989226626970683, 0.5394725726231028]]\n",
      "m = \n",
      "[-0.48625324  2.7350717   9.23249705]\n",
      "P = \n",
      "[[2.06762435 0.07175753 0.03286778]\n",
      " [0.07175753 0.11332778 0.01519814]\n",
      " [0.03286778 0.01519814 0.09091339]]\n",
      "num = \n",
      "522\n",
      "SENSOR 9 2.18268166452 -0.647873105786\n",
      "\n",
      "num = \n",
      "523\n",
      "SENSOR 3 2.60693896315 -2.37482011041\n",
      "\n",
      "num = \n",
      "524\n",
      "SENSOR 7 4.87407767625 -1.66971167459\n",
      "\n",
      "num = \n",
      "525\n",
      "ODOMETRY -0.0102628286794 0.0997605730161 -0.0210500632473\n",
      "\n",
      "sqP = \n",
      "[[1.437923625300608, 0.0, 0.0], [0.04990357429061785, 0.3329225341520278, 0.0], [0.02285780387338093, 0.042224388808470854, 0.2976709872748453]]\n",
      "sigma = \n",
      "[[-0.48625324  2.7350717   9.23249705]\n",
      " [ 2.38959401  2.7350717   9.23249705]\n",
      " [-0.38644609  3.40091677  9.23249705]\n",
      " [-0.44053763  2.81952048  9.82783903]\n",
      " [-3.36210049  2.7350717   9.23249705]\n",
      " [-0.58606039  2.06922663  9.23249705]\n",
      " [-0.53196884  2.65062292  8.63715508]]\n",
      "sigmax = \n",
      "[[-0.51651764  2.8229487   9.18493516]\n",
      " [ 2.35932961  2.66277056  9.3014684 ]\n",
      " [-0.41671049  3.49309558  9.19392805]\n",
      " [-0.47080203  2.90947923  9.78434277]\n",
      " [ 2.89082042  2.63778833  9.25531037]\n",
      " [-0.61632479  2.15192716  9.17641566]\n",
      " [-0.56223325  2.73623455  8.58562692]]\n",
      "sqP = \n",
      "[[1.4391729659535502, 0.0, 0.0], [-0.058863169476995596, 0.5625385760205117, 0.0], [0.042057525137796564, 0.024843872068725965, 0.5377806121268307]]\n",
      "m = \n",
      "[0.2697337  2.86061743 9.32067298]\n",
      "P = \n",
      "[[ 2.06758991 -0.06307046  0.04410049]\n",
      " [-0.06307046  0.11187447  0.00999265]\n",
      " [ 0.04410049  0.00999265  0.0918668 ]]\n",
      "num = \n",
      "526\n",
      "SENSOR 9 2.15683089899 -0.642555173461\n",
      "\n",
      "num = \n",
      "527\n",
      "SENSOR 3 2.6756869293 -2.29039615081\n",
      "\n",
      "num = \n",
      "528\n",
      "SENSOR 7 5.11674482285 -1.47951929626\n",
      "\n",
      "num = \n",
      "529\n",
      "ODOMETRY -0.0101962211399 0.099836539792 -0.0188237479265\n",
      "\n",
      "sqP = \n",
      "[[1.4379116486901184, 0.0, 0.0], [-0.043862543081513355, 0.3315879174157316, 0.0], [0.030669816653750463, 0.03419275392142657, 0.29959475919229]]\n",
      "sigma = \n",
      "[[ 0.2697337   2.86061743  9.32067298]\n",
      " [ 3.145557    2.86061743  9.32067298]\n",
      " [ 0.18200862  3.52379326  9.32067298]\n",
      " [ 0.33107334  2.92900293  9.9198625 ]\n",
      " [-2.60608959  2.86061743  9.32067298]\n",
      " [ 0.35745879  2.19744159  9.32067298]\n",
      " [ 0.20839407  2.79223192  8.72148346]]\n",
      "sigmax = \n",
      "[[ 0.23842081  2.9570386   9.34626847]\n",
      " [ 3.11424411  2.76085883  9.32130131]\n",
      " [ 0.15069573  3.62208615  9.33772233]\n",
      " [ 0.29976045  3.02367374  9.95132058]\n",
      " [-2.63740248  2.77430423  9.27065094]\n",
      " [ 0.3261459   2.2912495   9.35461775]\n",
      " [ 0.17708118  2.89004079  8.74112008]]\n",
      "sqP = \n",
      "[[1.5068027308768348, 0.0, 0.0], [-0.012212725695165376, 0.5642612108893501, 0.0], [0.018364789396250514, 0.018904634212555484, 0.5399013217118516]]\n",
      "m = \n",
      "[0.24361442 2.89755081 9.47555639]\n",
      "P = \n",
      "[[ 2.27010958 -0.0138534   0.02058512]\n",
      " [-0.0138534   0.11083691  0.01197562]\n",
      " [ 0.02058512  0.01197562  0.08933428]]\n",
      "num = \n",
      "530\n",
      "SENSOR 9 1.9915555629 -0.605343778173\n",
      "\n",
      "num = \n",
      "531\n",
      "SENSOR 3 2.8321255953 -2.36581198314\n",
      "\n",
      "num = \n",
      "532\n",
      "SENSOR 7 4.93146123799 -1.5890166699\n",
      "\n",
      "num = \n",
      "533\n",
      "ODOMETRY -0.0109072611821 0.100057984487 -0.0199246493126\n",
      "\n",
      "sqP = \n",
      "[[1.5066882828393369, 0.0, 0.0], [-0.009194602842132207, 0.3327947845434377, 0.0], [0.013662492171400663, 0.036362470449217595, 0.2963534789037209]]\n",
      "sigma = \n",
      "[[ 0.24361442  2.89755081  9.47555639]\n",
      " [ 3.25699099  2.89755081  9.47555639]\n",
      " [ 0.22522522  3.56314038  9.47555639]\n",
      " [ 0.27093941  2.97027575 10.06826334]\n",
      " [-2.76976214  2.89755081  9.47555639]\n",
      " [ 0.26200363  2.23196124  9.47555639]\n",
      " [ 0.21628944  2.82482587  8.88284943]]\n",
      "sigmax = \n",
      "[[ 0.21459445  2.99467993  9.49864901]\n",
      " [-3.05521429  2.79826623  9.46507273]\n",
      " [ 0.19620525  3.66067771  9.49685908]\n",
      " [ 0.24191944  3.06673768 10.09400107]\n",
      " [-2.79878211  2.80417171  9.44023389]\n",
      " [ 0.23298366  2.32864931  9.50043114]\n",
      " [ 0.18726947  2.92254966  8.90327971]]\n",
      "sqP = \n",
      "[[1.4335033557399928, 0.0, 0.0], [0.07776252200166327, 0.5596111154045857, 0.0], [0.021463793178526088, 0.018625435882616007, 0.5368816863447039]]\n",
      "m = \n",
      "[-0.55194741  3.07471268  9.51594621]\n",
      "P = \n",
      "[[2.05086852 0.08388588 0.02422894]\n",
      " [0.08388588 0.11969783 0.01599186]\n",
      " [0.02422894 0.01599186 0.08763282]]\n",
      "num = \n",
      "534\n",
      "SENSOR 9 2.05060986776 -0.812359999266\n",
      "\n",
      "num = \n",
      "535\n",
      "SENSOR 3 2.89469192638 -2.33538439474\n",
      "\n",
      "num = \n",
      "536\n",
      "SENSOR 7 5.06379731865 -1.57967642574\n",
      "\n",
      "num = \n",
      "537\n",
      "ODOMETRY -0.0110088218391 0.10014697699 -0.0202732496012\n",
      "\n",
      "sqP = \n",
      "[[1.4320853759443137, 0.0, 0.0], [0.05857603230867386, 0.3409789935568177, 0.0], [0.016918639030768325, 0.043993434247006656, 0.2922518762738978]]\n",
      "sigma = \n",
      "[[-0.55194741  3.07471268  9.51594621]\n",
      " [ 2.31222334  3.07471268  9.51594621]\n",
      " [-0.43479535  3.75667067  9.51594621]\n",
      " [-0.51811013  3.16269955 10.10044996]\n",
      " [-3.41611816  3.07471268  9.51594621]\n",
      " [-0.66909948  2.39275469  9.51594621]\n",
      " [-0.58578469  2.98672581  8.93144245]]\n",
      "sigmax = \n",
      "[[-0.58277932  3.15933525  9.462555  ]\n",
      " [ 2.28139143  3.0079483   9.59047216]\n",
      " [-0.46562726  3.84695379  9.47281201]\n",
      " [-0.54894204  3.24907995 10.04995216]\n",
      " [ 2.83623523  2.97870305  9.54411981]\n",
      " [-0.69993139  2.47055663  9.45302992]\n",
      " [-0.6166166   3.06949367  8.87521896]]\n",
      "sqP = \n",
      "[[1.4399683833327062, 0.0, 0.0], [-0.054003203701405794, 0.5680770162389086, 0.0], [0.04532786431197465, 0.024939641478710247, 0.5345875573440266]]\n",
      "m = \n",
      "[0.2131592  3.10396059 9.59235885]\n",
      "P = \n",
      "[[ 2.07006538 -0.05809865  0.04762747]\n",
      " [-0.05809865  0.11748848  0.01035093]\n",
      " [ 0.04762747  0.01035093  0.089264  ]]\n",
      "num = \n",
      "538\n",
      "SENSOR 9 1.9855033654 -0.729611380785\n",
      "\n",
      "num = \n",
      "539\n",
      "SENSOR 3 2.808098691 -2.40456966848\n",
      "\n",
      "num = \n",
      "540\n",
      "SENSOR 7 5.00506879142 -1.52174912725\n",
      "\n",
      "num = \n",
      "541\n",
      "ODOMETRY -0.0097077990525 0.100157141283 -0.0214692363299\n",
      "\n",
      "sqP = \n",
      "[[1.4387721771557576, 0.0, 0.0], [-0.0403807181698878, 0.34037901572194845, 0.0], [0.033102855095388316, 0.03433716231605871, 0.2949392567944564]]\n",
      "sigma = \n",
      "[[ 0.2131592   3.10396059  9.59235885]\n",
      " [ 3.09070355  3.10396059  9.59235885]\n",
      " [ 0.13239776  3.78471862  9.59235885]\n",
      " [ 0.27936491  3.17263491 10.18223736]\n",
      " [-2.66438516  3.10396059  9.59235885]\n",
      " [ 0.29392063  2.42320256  9.59235885]\n",
      " [ 0.14695349  3.03528626  9.00248033]]\n",
      "sigmax = \n",
      "[[ 0.18187712  3.20206828  9.61246599]\n",
      " [ 3.05942148  3.0040054   9.59855378]\n",
      " [ 0.10111569  3.88412866  9.60448575]\n",
      " [ 0.24808283  3.26919744 10.208791  ]\n",
      " [-2.69566723  3.01450095  9.54734339]\n",
      " [ 0.26263856  2.51936836  9.62031516]\n",
      " [ 0.11567141  3.13450926  9.01605288]]\n",
      "sqP = \n",
      "[[1.5075781171046827, 0.0, 0.0], [-0.01090360602594675, 0.5695528188971712, 0.0], [0.018871711698013452, 0.01806413468880083, 0.5373112604473498]]\n",
      "m = \n",
      "[0.18087813 3.14221566 9.56846424]\n",
      "P = \n",
      "[[ 2.27245191 -0.01237838  0.02112089]\n",
      " [-0.01237838  0.11893401  0.01355234]\n",
      " [ 0.02112089  0.01355234  0.08630906]]\n",
      "num = \n",
      "542\n",
      "SENSOR 9 1.85666944851 -0.732607970484\n",
      "\n",
      "num = \n",
      "543\n",
      "SENSOR 3 3.15325419611 -2.35565991968\n",
      "\n",
      "num = \n",
      "544\n",
      "SENSOR 7 4.77716786006 -1.61242538926\n",
      "\n",
      "num = \n",
      "545\n",
      "ODOMETRY -0.00981948245873 0.0999149565253 -0.020711936978\n",
      "\n",
      "sqP = \n",
      "[[1.5074653912833824, 0.0, 0.0], [-0.008211384394416607, 0.34477033494858894, 0.0], [0.014010862059755304, 0.03964201818598533, 0.290759814851497]]\n",
      "sigma = \n",
      "[[ 0.18087813  3.14221566  9.56846424]\n",
      " [ 3.19580891  3.14221566  9.56846424]\n",
      " [ 0.16445536  3.83175633  9.56846424]\n",
      " [ 0.20889985  3.2214997  10.14998387]\n",
      " [-2.83405266  3.14221566  9.56846424]\n",
      " [ 0.1973009   2.45267499  9.56846424]\n",
      " [ 0.1528564   3.06293163  8.98694461]]\n",
      "sigmax = \n",
      "[[ 0.14970109  3.24090912  9.58552458]\n",
      " [-3.11855343  3.04215771  9.56400787]\n",
      " [ 0.13327832  3.93071664  9.58390153]\n",
      " [ 0.17772282  3.31967641 10.16980271]\n",
      " [-2.86522969  3.04646795  9.53907328]\n",
      " [ 0.16612386  2.55107497  9.58714302]\n",
      " [ 0.12167937  3.16206433  9.00123305]]\n",
      "sqP = \n",
      "[[1.4334662059798786, 0.0, 0.0], [0.07923637886419062, 0.5669957373811425, 0.0], [0.01654835094764215, 0.019693849998450648, 0.5337991359378128]]\n",
      "m = \n",
      "[-0.61244775  3.35252552  9.60724022]\n",
      "P = \n",
      "[[2.0507199  0.08511449 0.01880375]\n",
      " [0.08511449 0.12373713 0.01417677]\n",
      " [0.01880375 0.01417677 0.08613879]]\n",
      "num = \n",
      "546\n",
      "SENSOR 9 1.66695280875 -0.677940267447\n",
      "\n",
      "num = \n",
      "547\n",
      "SENSOR 3 2.93583551959 -2.29721883235\n",
      "\n",
      "num = \n",
      "548\n",
      "SENSOR 7 4.77792363509 -1.55059855566\n",
      "\n",
      "num = \n",
      "549\n",
      "ODOMETRY -0.00992896039159 0.0998189682803 -0.0204511862369\n",
      "\n",
      "sqP = \n",
      "[[1.432033483913475, 0.0, 0.0], [0.05943610430100479, 0.3467051745014995, 0.0], [0.013130804875029649, 0.03863896720904192, 0.29064307587703225]]\n",
      "sigma = \n",
      "[[-0.61244775  3.35252552  9.60724022]\n",
      " [ 2.25161921  3.35252552  9.60724022]\n",
      " [-0.49357555  4.04593587  9.60724022]\n",
      " [-0.58618614  3.42980345 10.18852637]\n",
      " [-3.47651472  3.35252552  9.60724022]\n",
      " [-0.73131996  2.65911517  9.60724022]\n",
      " [-0.63870936  3.27524758  9.02595406]]\n",
      "sigmax = \n",
      "[[-0.64297917  3.43371232  9.54900189]\n",
      " [ 2.22108779  3.29040118  9.68549343]\n",
      " [-0.52410697  4.13345636  9.55904102]\n",
      " [-0.61671756  3.51249151 10.13243998]\n",
      " [ 2.77613917  3.25848927  9.64100682]\n",
      " [-0.76185138  2.73282241  9.53978474]\n",
      " [-0.66924078  3.35487713  8.96560397]]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "sqP = \n",
      "[[1.4399691039393638, 0.0, 0.0], [-0.05102585687728423, 0.5712861295261777, 0.0], [0.048463785469211436, 0.021597755000637278, 0.5337293830703919]]\n",
      "m = \n",
      "[0.13019699 3.45884058 9.5098038 ]\n",
      "P = \n",
      "[[ 2.07011174 -0.05514392  0.05112509]\n",
      " [-0.05514392  0.12177305  0.00801516]\n",
      " [ 0.05112509  0.00801516  0.08828333]]\n",
      "num = \n",
      "550\n",
      "SENSOR 9 1.71819656468 -0.707245929411\n",
      "\n",
      "num = \n",
      "551\n",
      "SENSOR 3 3.02406315637 -2.3968858507\n",
      "\n",
      "num = \n",
      "552\n",
      "SENSOR 7 4.90173372979 -1.65060778383\n",
      "\n",
      "num = \n",
      "553\n",
      "ODOMETRY -0.00884110158486 0.100015505449 -0.0206146468981\n",
      "\n",
      "sqP = \n",
      "[[1.4387882902864595, 0.0, 0.0], [-0.03832663949180345, 0.34684883831853625, 0.0], [0.03553343324795152, 0.027034928824673007, 0.29375128532900835]]\n",
      "sigma = \n",
      "[[ 0.13019699  3.45884058  9.5098038 ]\n",
      " [ 3.00777357  3.45884058  9.5098038 ]\n",
      " [ 0.05354371  4.15253826  9.5098038 ]\n",
      " [ 0.20126386  3.51291044 10.09730637]\n",
      " [-2.74737959  3.45884058  9.5098038 ]\n",
      " [ 0.20685027  2.76514291  9.5098038 ]\n",
      " [ 0.05913013  3.40477073  8.92230123]]\n",
      "sigmax = \n",
      "[[ 0.09981685  3.55793851  9.52177991]\n",
      " [ 2.97739343  3.36005115  9.52410322]\n",
      " [ 0.02316357  4.2522623   9.514156  ]\n",
      " [ 0.17088371  3.61090784 10.11628891]\n",
      " [-2.77775973  3.36630171  9.47238212]\n",
      " [ 0.17647013  2.86303273  9.52933349]\n",
      " [ 0.02874998  3.5044689   8.92721046]]\n",
      "sqP = \n",
      "[[1.507595204436043, 0.0, 0.0], [-0.009693516252315846, 0.5730079631489668, 0.0], [0.019443126883988804, 0.013295905237297321, 0.5367491762788171]]\n",
      "m = \n",
      "[0.10247553 3.4819304  9.57588651]\n",
      "P = \n",
      "[[ 2.27251369 -0.01108225  0.02194752]\n",
      " [-0.01108225  0.11825011  0.00677573]\n",
      " [ 0.02194752  0.00677573  0.08736283]]\n",
      "num = \n",
      "554\n",
      "SENSOR 9 1.70981171667 -0.684234093975\n",
      "\n",
      "num = \n",
      "555\n",
      "SENSOR 3 3.16299858885 -2.37554434755\n",
      "\n",
      "num = \n",
      "556\n",
      "SENSOR 7 5.05446186142 -1.55954853363\n",
      "\n",
      "num = \n",
      "557\n",
      "ODOMETRY -0.0102024861503 0.10032005589 -0.0183984058531\n",
      "\n",
      "sqP = \n",
      "[[1.5074858827700586, 0.0, 0.0], [-0.0073514806714433415, 0.3437965528426641, 0.0], [0.014559024522422831, 0.02001985084568386, 0.29453365737021064]]\n",
      "sigma = \n",
      "[[ 0.10247553  3.4819304   9.57588651]\n",
      " [ 3.11744729  3.4819304   9.57588651]\n",
      " [ 0.08777257  4.16952351  9.57588651]\n",
      " [ 0.13159358  3.5219701  10.16495382]\n",
      " [-2.91249624  3.4819304   9.57588651]\n",
      " [ 0.11717849  2.79433729  9.57588651]\n",
      " [ 0.07335748  3.4418907   8.98681919]]\n",
      "sigmax = \n",
      "[[ 0.07301978  3.58150779  9.58523772]\n",
      " [ 3.08799154  3.3819693   9.57918507]\n",
      " [ 0.05831682  4.26922762  9.58377268]\n",
      " [ 0.10213783  3.62123303 10.17720016]\n",
      " [-2.94195199  3.3843311   9.55403524]\n",
      " [ 0.08772274  2.89376643  9.58670074]\n",
      " [ 0.04390173  3.54169813  8.99326735]]\n",
      "sqP = \n",
      "[[1.572507455058363, 0.0, 0.0], [-0.0019895135197371804, 0.5709738226367781, 0.0], [0.008771252211417535, 0.011103742755569017, 0.5362004702406191]]\n",
      "m = \n",
      "[0.07509229 3.52026241 9.70424987]\n",
      "P = \n",
      "[[ 2.47272260e+00 -2.39333927e-03  1.04736129e-02]\n",
      " [-2.39333927e-03  1.20910767e-01  7.57114680e-03]\n",
      " [ 1.04736129e-02  7.57114680e-03  8.59563152e-02]]\n",
      "num = \n",
      "558\n",
      "SENSOR 9 1.48285099536 -0.732863940447\n",
      "\n",
      "num = \n",
      "559\n",
      "SENSOR 3 3.24066838872 -2.3472192049\n",
      "\n",
      "num = \n",
      "560\n",
      "SENSOR 7 5.02189688945 -1.52629273662\n",
      "\n",
      "num = \n",
      "561\n",
      "ODOMETRY -0.0095354970248 0.100025384548 -0.0207858100942\n",
      "\n",
      "sqP = \n",
      "[[1.5724892998849962, 0.0, 0.0], [-0.0015220067127368828, 0.3477189239342389, 0.0], [0.006660530488393418, 0.02180290934683918, 0.29229537408543405]]\n",
      "sigma = \n",
      "[[ 0.07509229  3.52026241  9.70424987]\n",
      " [ 3.22007089  3.52026241  9.70424987]\n",
      " [ 0.07204827  4.21570025  9.70424987]\n",
      " [ 0.08841335  3.56386822 10.28884061]\n",
      " [-3.06988631  3.52026241  9.70424987]\n",
      " [ 0.0781363   2.82482456  9.70424987]\n",
      " [ 0.06177123  3.47665659  9.11965912]]\n",
      "sigmax = \n",
      "[[ 0.0464914   3.62037133  9.71075505]\n",
      " [-3.09171531  3.42017608  9.69740576]\n",
      " [ 0.04344738  4.31582851  9.71045028]\n",
      " [ 0.05981246  3.66388161 10.29667874]\n",
      " [-3.0984872   3.42013203  9.69808369]\n",
      " [ 0.04953541  2.92491321  9.71105975]\n",
      " [ 0.03317034  3.57684328  9.1248302 ]]\n",
      "sqP = \n",
      "[[1.431991749463997, 0.0, 0.0], [0.08208956189380195, 0.5675968544837573, 0.0], [0.006714300507080015, 0.011027153735339381, 0.5345015623583063]]\n",
      "m = \n",
      "[-0.72214043  3.68457315  9.73271947]\n",
      "P = \n",
      "[[2.0465024  0.08931693 0.0081018 ]\n",
      " [0.08931693 0.13348672 0.00973429]\n",
      " [0.0081018  0.00973429 0.0837373 ]]\n",
      "num = \n",
      "562\n",
      "SENSOR 9 1.46150131465 -0.767666096771\n",
      "\n",
      "num = \n",
      "563\n",
      "SENSOR 3 3.22029667183 -2.36535752613\n",
      "\n",
      "num = \n",
      "564\n",
      "SENSOR 7 4.84479242128 -1.46298563412\n",
      "\n",
      "num = \n",
      "565\n",
      "ODOMETRY -0.00906969760687 0.099796851015 -0.0208710090182\n",
      "\n",
      "sqP = \n",
      "[[1.4305601701455302, 0.0, 0.0], [0.062434935802270705, 0.3599841652892818, 0.0], [0.005663378653920388, 0.02605864952281946, 0.28814262100056875]]\n",
      "sigma = \n",
      "[[-0.72214043  3.68457315  9.73271947]\n",
      " [ 2.13897991  3.68457315  9.73271947]\n",
      " [-0.59727056  4.40454148  9.73271947]\n",
      " [-0.71081367  3.73669045 10.30900471]\n",
      " [-3.58326077  3.68457315  9.73271947]\n",
      " [-0.8470103   2.96460482  9.73271947]\n",
      " [-0.73346719  3.63245585  9.15643423]]\n",
      "sigmax = \n",
      "[[-0.75246174  3.75899761  9.66589076]\n",
      " [ 2.1086586   3.63155567  9.81753824]\n",
      " [-0.62759186  4.48670969  9.67568033]\n",
      " [-0.74113498  3.81186707 10.24302326]\n",
      " [ 2.66960323  3.59455802  9.77633544]\n",
      " [-0.87733161  3.03012658  9.65714185]\n",
      " [-0.76378849  3.7061186   9.08876683]]\n",
      "sqP = \n",
      "[[1.4401899874330646, 0.0, 0.0], [-0.044621510837872844, 0.5791186863289552, 0.0], [0.05385996765634926, 0.014226189530700312, 0.5324518338547946]]\n",
      "m = \n",
      "[0.02426083 3.77209268 9.65185002]\n",
      "P = \n",
      "[[ 2.07083031 -0.04874801  0.05701491]\n",
      " [-0.04874801  0.13469238  0.00485696]\n",
      " [ 0.05701491  0.00485696  0.08673997]]\n",
      "num = \n",
      "566\n",
      "SENSOR 9 1.53472735309 -0.701353506585\n",
      "\n",
      "num = \n",
      "567\n",
      "SENSOR 3 3.30563763836 -2.28920291979\n",
      "\n",
      "num = \n",
      "568\n",
      "SENSOR 7 5.08447465054 -1.48707316086\n",
      "\n",
      "num = \n",
      "569\n",
      "ODOMETRY -0.00996926665775 0.100146565387 -0.018774822561\n",
      "\n",
      "sqP = \n",
      "[[1.439037981325748, 0.0, 0.0], [-0.03387541910215787, 0.36543786326884586, 0.0], [0.039620156597567895, 0.016963502064621945, 0.29134593882212634]]\n",
      "sigma = \n",
      "[[ 0.02426083  3.77209268  9.65185002]\n",
      " [ 2.90233679  3.77209268  9.65185002]\n",
      " [-0.04349001  4.5029684   9.65185002]\n",
      " [ 0.10350114  3.80601968 10.2345419 ]\n",
      " [-2.85381514  3.77209268  9.65185002]\n",
      " [ 0.09201166  3.04121695  9.65185002]\n",
      " [-0.05497949  3.73816567  9.06915814]]\n",
      "sigmax = \n",
      "[[-5.67988009e-03  3.87187801e+00  9.65336599e+00]\n",
      " [ 2.87239608e+00  3.67535706e+00  9.67637821e+00]\n",
      " [-7.34307183e-02  4.60262744e+00  9.64660714e+00]\n",
      " [ 7.35604331e-02  3.90537190e+00  1.02439519e+01]\n",
      " [-2.88375584e+00  3.67614681e+00  9.62439455e+00]\n",
      " [ 6.20709581e-02  3.14067073e+00  9.66011788e+00]\n",
      " [-8.49201933e-02  3.83775789e+00  9.06277061e+00]]\n",
      "sqP = \n",
      "[[1.5078287742732084, 0.0, 0.0], [-0.007955479028344963, 0.5839553212455609, 0.0], [0.020238161515881873, 0.006892351719055751, 0.5356530580073382]]\n",
      "m = \n",
      "[-5.39170400e-04  3.77381329e+00  9.77050227e+00]\n",
      "P = \n",
      "[[ 2.27321853 -0.00919419  0.02285132]\n",
      " [-0.00919419  0.13513175  0.00375208]\n",
      " [ 0.02285132  0.00375208  0.0854217 ]]\n",
      "num = \n",
      "570\n",
      "SENSOR 9 1.27538476788 -0.825565964241\n",
      "\n",
      "num = \n",
      "571\n",
      "SENSOR 3 3.52081304084 -2.35140411346\n",
      "\n",
      "num = \n",
      "572\n",
      "SENSOR 7 4.9713536015 -1.50506099629\n",
      "\n",
      "num = \n",
      "573\n",
      "ODOMETRY -0.00982937584718 0.100296415285 -0.0202122708831\n",
      "\n",
      "sqP = \n",
      "[[1.507719645352572, 0.0, 0.0], [-0.00609807940746769, 0.36755211667201515, 0.0], [0.015156213320396383, 0.01045974322224225, 0.29168918923823817]]\n",
      "sigma = \n",
      "[[-5.39170400e-04  3.77381329e+00  9.77050227e+00]\n",
      " [ 3.01490012e+00  3.77381329e+00  9.77050227e+00]\n",
      " [-1.27353292e-02  4.50891752e+00  9.77050227e+00]\n",
      " [ 2.97732562e-02  3.79473278e+00  1.03538807e+01]\n",
      " [-3.01597846e+00  3.77381329e+00  9.77050227e+00]\n",
      " [ 1.16569884e-02  3.03870906e+00  9.77050227e+00]\n",
      " [-3.08515970e-02  3.75289380e+00  9.18712389e+00]]\n",
      "sigmax = \n",
      "[[-2.92832596e-02  3.87395433e+00  9.76944991e+00]\n",
      " [ 2.98615603e+00  3.67460046e+00  9.78414592e+00]\n",
      " [-4.14794184e-02  4.60903828e+00  9.76822868e+00]\n",
      " [ 1.02916702e-03  3.89485970e+00  1.03558638e+01]\n",
      " [-3.04472255e+00  3.67433565e+00  9.75894663e+00]\n",
      " [-1.70871008e-02  3.13885548e+00  9.77067129e+00]\n",
      " [-5.95956863e-02  3.85295694e+00  9.18303696e+00]]\n",
      "sqP = \n",
      "[[1.5727318354877793, 0.0, 0.0], [-0.0012606945099937212, 0.5853701254141018, 0.0], [0.008867369659227103, 0.00474964166809509, 0.5347054079975924]]\n",
      "m = \n",
      "[-0.02925665  4.00425729  9.79495034]\n",
      "P = \n",
      "[[ 2.47342827e+00 -1.50459321e-03  1.05656536e-02]\n",
      " [-1.50459321e-03  1.41770012e-01  5.50906811e-03]\n",
      " [ 1.05656536e-02  5.50906811e-03  8.27656420e-02]]\n",
      "num = \n",
      "574\n",
      "SENSOR 9 1.28245800998 -0.820894690375\n",
      "\n",
      "num = \n",
      "575\n",
      "SENSOR 3 3.61786607428 -2.28348332028\n",
      "\n",
      "num = \n",
      "576\n",
      "SENSOR 7 4.72683380487 -1.5245034595\n",
      "\n",
      "num = \n",
      "577\n",
      "ODOMETRY -0.00967296493409 0.100095993623 -0.0208377359394\n",
      "\n",
      "sqP = \n",
      "[[1.572713665460134, 0.0, 0.0], [-0.0009566860410589958, 0.37652237232699765, 0.0], [0.00671810378557427, 0.014648519276411684, 0.2872384548223131]]\n",
      "sigma = \n",
      "[[-0.02925665  4.00425729  9.79495034]\n",
      " [ 3.11617068  4.00425729  9.79495034]\n",
      " [-0.03117002  4.75730204  9.79495034]\n",
      " [-0.01582044  4.03355433 10.36942725]\n",
      " [-3.17468398  4.00425729  9.79495034]\n",
      " [-0.02734328  3.25121255  9.79495034]\n",
      " [-0.04269286  3.97496025  9.22047343]]\n",
      "sigmax = \n",
      "[[-0.0592983   4.10447711  9.79103115]\n",
      " [ 3.08612903  3.90402319  9.79848519]\n",
      " [-0.06121167  4.85751417  9.7908394 ]\n",
      " [-0.04586209  4.13381776 10.36685495]\n",
      " [ 3.07845968  3.90405325  9.79925381]\n",
      " [-0.05738493  3.35143968  9.79122292]\n",
      " [-0.0727345   4.07511836  9.21520806]]\n",
      "sqP = \n",
      "[[1.4319918119732622, 0.0, 0.0], [-0.08263181072637782, 0.5853941663471088, 0.0], [0.004574777220670511, 0.007298347381886003, 0.5318181368941577]]\n",
      "m = \n",
      "[0.70877693 4.17261035 9.73303175]\n",
      "P = \n",
      "[[ 2.04683077e+00 -9.17453276e-02  4.89373445e-03]\n",
      " [-9.17453276e-02  1.60948868e-01  6.33109746e-04]\n",
      " [ 4.89373445e-03  6.33109746e-04  8.19747726e-02]]\n",
      "num = \n",
      "578\n",
      "SENSOR 9 1.22717122688 -0.834555116798\n",
      "\n",
      "num = \n",
      "579\n",
      "SENSOR 3 3.46599411335 -2.26643278842\n",
      "\n",
      "num = \n",
      "580\n",
      "SENSOR 7 4.77048897684 -1.61915796579\n",
      "\n",
      "num = \n",
      "581\n",
      "ODOMETRY -0.00983012240915 0.100048682167 -0.0200418409326\n",
      "\n",
      "sqP = \n",
      "[[1.4306749359075266, 0.0, 0.0], [-0.06412730475786099, 0.39602595414189545, 0.0], [0.0034205774663864197, 0.002152541143093114, 0.28628384316882904]]\n",
      "sigma = \n",
      "[[ 0.70877693  4.17261035  9.73303175]\n",
      " [ 3.5701268   4.17261035  9.73303175]\n",
      " [ 0.58052232  4.96466226  9.73303175]\n",
      " [ 0.71561808  4.17691544 10.30559944]\n",
      " [-2.15257294  4.17261035  9.73303175]\n",
      " [ 0.83703154  3.38055844  9.73303175]\n",
      " [ 0.70193577  4.16830527  9.16046407]]\n",
      "sigmax = \n",
      "[[ 0.67826623  4.24922574  9.79744674]\n",
      " [-2.74356921  4.08116739  9.69232068]\n",
      " [ 0.55001162  5.04888727  9.78711831]\n",
      " [ 0.68510738  4.25308836 10.37053705]\n",
      " [-2.18308364  4.11680033  9.64993873]\n",
      " [ 0.80652084  3.44830567  9.80671703]\n",
      " [ 0.67142507  4.24535953  9.22435341]]\n",
      "sqP = \n",
      "[[1.4402461043009895, 0.0, 0.0], [0.04441634067883302, 0.6020358858193605, 0.0], [0.0514581491014043, -0.000395981117293345, 0.5315826890894304]]\n",
      "m = \n",
      "[-0.11709384  4.1853125   9.68283593]\n",
      "P = \n",
      "[[ 2.0713058   0.04911583  0.05442233]\n",
      " [ 0.04911583  0.16988766 -0.00314236]\n",
      " [ 0.05442233 -0.00314236  0.08589053]]\n",
      "num = \n",
      "582\n",
      "SENSOR 9 1.21052101312 -0.846681880547\n",
      "\n",
      "num = \n",
      "583\n",
      "SENSOR 3 3.70451197871 -2.25861912203\n",
      "\n",
      "num = \n",
      "584\n",
      "SENSOR 7 5.07806532139 -1.49123903126\n",
      "\n",
      "num = \n",
      "585\n",
      "ODOMETRY -0.00930456459578 0.100071988622 -0.0204347040085\n",
      "\n",
      "sqP = \n",
      "[[1.439203180976894, 0.0, 0.0], [0.034127096071654664, 0.4107590573813658, 0.0], [0.0378142087339133, -0.010791839988056388, 0.29042064921684907]]\n",
      "sigma = \n",
      "[[-0.11709384  4.1853125   9.68283593]\n",
      " [ 2.76131252  4.1853125   9.68283593]\n",
      " [-0.04883965  5.00683062  9.68283593]\n",
      " [-0.04146542  4.16372882 10.26367723]\n",
      " [-2.9955002   4.1853125   9.68283593]\n",
      " [-0.18534803  3.36379439  9.68283593]\n",
      " [-0.19272226  4.20689618  9.10199463]]\n",
      "sigmax = \n",
      "[[-0.1469658   4.28455639  9.67017142]\n",
      " [ 2.73144056  4.09278077  9.72088348]\n",
      " [-0.07871161  5.10670716  9.67696946]\n",
      " [-0.07133739  4.26364591 10.25854743]\n",
      " [-3.02537216  4.08619121  9.66924521]\n",
      " [-0.21522     3.46194346  9.66343236]\n",
      " [-0.22259422  4.3048995   9.08186782]]\n",
      "sqP = \n",
      "[[1.5079457447807816, 0.0, 0.0], [0.010619537971751138, 0.6133486640727015, 0.0], [0.019774443660386092, -0.004467136865218862, 0.535181314274648]]\n",
      "m = \n",
      "[-0.14000538  4.27606728  9.84519534]\n",
      "P = \n",
      "[[ 2.27355536  0.0122051   0.02229535]\n",
      " [ 0.0122051   0.16967215 -0.00875376]\n",
      " [ 0.02229535 -0.00875376  0.08691186]]\n",
      "num = \n",
      "586\n",
      "SENSOR 9 1.12120365478 -0.935390860361\n",
      "\n",
      "num = \n",
      "587\n",
      "SENSOR 3 3.58620574347 -2.27331836708\n",
      "\n",
      "num = \n",
      "588\n",
      "SENSOR 7 4.76861242282 -1.53629792407\n",
      "\n",
      "num = \n",
      "589\n",
      "ODOMETRY 0.000468699390922 0.0998304456128 -2.5849995276e-05\n",
      "\n",
      "sqP = \n",
      "[[1.5078313420103493, 0.0, 0.0], [0.00809447260017408, 0.4118332524823933, 0.0], [0.014786367945785167, -0.021546209000765468, 0.2936477139988997]]\n",
      "sigma = \n",
      "[[-0.14000538  4.27606728  9.84519534]\n",
      " [ 2.8756573   4.27606728  9.84519534]\n",
      " [-0.12381643  5.09973379  9.84519534]\n",
      " [-0.11043264  4.23297486 10.43249077]\n",
      " [-3.15566806  4.27606728  9.84519534]\n",
      " [-0.15619433  3.45240078  9.84519534]\n",
      " [-0.16957812  4.3191597   9.25789992]]\n",
      "sigmax = \n",
      "[[-0.16974465  4.37502587  9.83030906]\n",
      " [ 2.84591804  4.179762    9.87239269]\n",
      " [-0.1535557   5.19892039  9.83191297]\n",
      " [-0.14017191  4.33233034 10.42053704]\n",
      " [ 3.09777797  4.17602264  9.84753481]\n",
      " [-0.18593359  3.55110541  9.82870904]\n",
      " [-0.19931738  4.41763485  9.24009409]]\n",
      "sqP = \n",
      "[[1.4334572095730223, 0.0, 0.0], [-0.07893493417637815, 0.6092749886878096, 0.0], [0.014960475792289814, -0.009638984298840051, 0.5357030332365963]]\n",
      "m = \n",
      "[0.61169575 4.3319513  9.71459644]\n",
      "P = \n",
      "[[ 2.05150176 -0.08862387  0.01674396]\n",
      " [-0.08862387  0.18732017 -0.01119377]\n",
      " [ 0.01674396 -0.01119377  0.08266582]]\n",
      "num = \n",
      "590\n",
      "SENSOR 9 1.14054428171 -0.998313577557\n",
      "\n",
      "num = \n",
      "591\n",
      "SENSOR 3 3.55052396622 -2.27149420334\n",
      "\n",
      "num = \n",
      "592\n",
      "SENSOR 6 4.97342840955 -0.481153301138\n",
      "\n",
      "num = \n",
      "593\n",
      "SENSOR 7 4.93140206369 -1.5097071492\n",
      "\n",
      "num = \n",
      "594\n",
      "ODOMETRY -0.000306172617703 0.0998659733612 0.00088045160042\n",
      "\n",
      "sqP = \n",
      "[[1.4323064492489945, 0.0, 0.0], [-0.061874938725461515, 0.4283592691282873, 0.0], [0.011690206558404257, -0.024443118696651802, 0.28623712945198576]]\n",
      "sigma = \n",
      "[[ 0.61169575  4.3319513   9.71459644]\n",
      " [ 3.47630865  4.3319513   9.71459644]\n",
      " [ 0.48794587  5.18866983  9.71459644]\n",
      " [ 0.63507616  4.28306506 10.2870707 ]\n",
      " [-2.25291715  4.3319513   9.71459644]\n",
      " [ 0.73544563  3.47523276  9.71459644]\n",
      " [ 0.58831534  4.38083753  9.14212219]]\n",
      "sigmax = \n",
      "[[ 0.6121386   4.41365315  9.77196303]\n",
      " [-2.80643381  4.23767646  9.68175786]\n",
      " [ 0.48838872  5.27682789  9.76143953]\n",
      " [ 0.63551901  4.36340345 10.34633166]\n",
      " [-2.2524743   4.26905045  9.63707483]\n",
      " [ 0.73588848  3.54922881  9.78160914]\n",
      " [ 0.58875819  4.46385819  9.19756304]]\n",
      "sqP = \n",
      "[[1.4400331386320908, 0.0, 0.0], [0.04747130369374207, 0.6243241207493202, 0.0], [0.04744929186997752, -0.01338552119381946, 0.5315708771334154]]\n",
      "m = \n",
      "[-0.17677314  4.33229965  9.73946814]\n",
      "P = \n",
      "[[2.07053966 0.0477379  0.05091486]\n",
      " [0.0477379  0.1055389  0.00663665]\n",
      " [0.05091486 0.00663665 0.08338528]]\n",
      "num = \n",
      "595\n",
      "SENSOR 9 0.908943386111 -1.05078434189\n",
      "\n",
      "num = \n",
      "596\n",
      "SENSOR 3 3.92854556221 -2.37317280336\n",
      "\n",
      "num = \n",
      "597\n",
      "SENSOR 6 4.65017824828 -0.48503026696\n",
      "\n",
      "num = \n",
      "598\n",
      "SENSOR 7 4.82256937785 -1.46085672931\n",
      "\n",
      "num = \n",
      "599\n",
      "ODOMETRY 6.35865396476e-05 0.100145996251 0.000825376841077\n",
      "\n",
      "sqP = \n",
      "[[1.438936990854515, 0.0, 0.0], [0.033175812311523306, 0.3231690955828654, 0.0], [0.03538366233952569, 0.016903753351143854, 0.2860900928346909]]\n",
      "sigma = \n",
      "[[-0.17677314  4.33229965  9.73946814]\n",
      " [ 2.70110085  4.33229965  9.73946814]\n",
      " [-0.11042151  4.97863784  9.73946814]\n",
      " [-0.10600581  4.36610715 10.31164832]\n",
      " [-3.05464712  4.33229965  9.73946814]\n",
      " [-0.24312476  3.68596146  9.73946814]\n",
      " [-0.24754046  4.29849214  9.16728795]]\n",
      "sigmax = \n",
      "[[-0.17619886  4.43060395  9.72187622]\n",
      " [ 2.70167512  4.24197971  9.78207709]\n",
      " [-0.10984723  5.07789223  9.72843279]\n",
      " [-0.10543153  4.4654093  10.30105136]\n",
      " [-3.05407284  4.23280825  9.73082663]\n",
      " [-0.24255048  3.78288305  9.71539706]\n",
      " [-0.24696618  4.3953065   9.14278914]]\n",
      "sqP = \n",
      "[[1.5076114558242564, 0.0, 0.0], [0.009724094718124254, 0.5585889689066903, 0.0], [0.019096796892136603, 0.008455292891091, 0.5327387822253412]]\n",
      "m = \n",
      "[-0.17097681  4.64374505  9.74934137]\n",
      "P = \n",
      "[[2.27257123 0.01096286 0.0215735 ]\n",
      " [0.01096286 0.09577823 0.00684238]\n",
      " [0.0215735  0.00684238 0.08040433]]\n",
      "num = \n",
      "600\n",
      "SENSOR 9 0.827243169323 -1.09325181967\n",
      "\n",
      "num = \n",
      "601\n",
      "SENSOR 3 3.73413074447 -2.37141305415\n",
      "\n",
      "num = \n",
      "602\n",
      "SENSOR 6 4.78905396728 -0.408920738147\n",
      "\n",
      "num = \n",
      "603\n",
      "SENSOR 7 4.99209306977 -1.55682191086\n",
      "\n",
      "num = \n",
      "604\n",
      "ODOMETRY 0.000201838205791 0.100136187552 -0.00014387330981\n",
      "\n",
      "sqP = \n",
      "[[1.5075049672920666, 0.0, 0.0], [0.007272186104473801, 0.3093951276299237, 0.0], [0.014310733724601428, 0.02177898963095286, 0.282356526534639]]\n",
      "sigma = \n",
      "[[-0.17097681  4.64374505  9.74934137]\n",
      " [ 2.84403312  4.64374505  9.74934137]\n",
      " [-0.15643244  5.26253531  9.74934137]\n",
      " [-0.14235535  4.68730303 10.31405443]\n",
      " [-3.18598675  4.64374505  9.74934137]\n",
      " [-0.18552118  4.0249548   9.74934137]\n",
      " [-0.19959828  4.60018707  9.18462832]]\n",
      "sigmax = \n",
      "[[-0.17008785  4.74243191  9.73230831]\n",
      " [ 2.84492209  4.54799812  9.77869688]\n",
      " [-0.15554348  5.36145945  9.7337454 ]\n",
      " [-0.14146638  4.78643691 10.29985252]\n",
      " [ 3.09808752  4.54369744  9.75377944]\n",
      " [-0.18463222  4.12338349  9.73087482]\n",
      " [-0.19870932  4.69834606  9.16477806]]\n",
      "sqP = \n",
      "[[1.433462348017012, 0.0, 0.0], [-0.07903216343706046, 0.5452401548389418, 0.0], [0.016503049324554268, 0.011980089234835418, 0.5294939970524319]]\n",
      "m = \n",
      "[0.62884873 4.60483061 9.76165784]\n",
      "P = \n",
      "[[ 2.05043144e+00 -8.45483170e-02  1.73617779e-02]\n",
      " [-8.45483170e-02  1.04149117e-01  6.05544239e-04]\n",
      " [ 1.73617779e-02  6.05544239e-04  7.85944008e-02]]\n",
      "num = \n",
      "605\n",
      "SENSOR 9 0.854965120184 -1.30243719417\n",
      "\n",
      "num = \n",
      "606\n",
      "SENSOR 3 4.08986785793 -2.31375803278\n",
      "\n",
      "num = \n",
      "607\n",
      "SENSOR 6 4.49746631975 -0.415197960969\n",
      "\n",
      "num = \n",
      "608\n",
      "SENSOR 7 4.91878504334 -1.53841931157\n",
      "\n",
      "num = \n",
      "609\n",
      "ODOMETRY 0.000309566514052 0.099997143531 0.000424327411615\n",
      "\n",
      "sqP = \n",
      "[[1.4319327622244187, 0.0, 0.0], [-0.059044893182848515, 0.31727404205316895, 0.0], [0.01212471589777033, 0.004165001289350231, 0.28005364625408746]]\n",
      "sigma = \n",
      "[[ 0.62884873  4.60483061  9.76165784]\n",
      " [ 3.49271426  4.60483061  9.76165784]\n",
      " [ 0.51075895  5.23937869  9.76165784]\n",
      " [ 0.65309816  4.61316061 10.32176514]\n",
      " [-2.23501679  4.60483061  9.76165784]\n",
      " [ 0.74693852  3.97028253  9.76165784]\n",
      " [ 0.6045993   4.59650061  9.20155055]]\n",
      "sigmax = \n",
      "[[ 0.6289067   4.68579938  9.8205757 ]\n",
      " [-2.79041309  4.51081093  9.7271969 ]\n",
      " [ 0.51081691  5.32672499  9.81062599]\n",
      " [ 0.65315613  4.69267699 10.38262893]\n",
      " [-2.23495883  4.54311802  9.68279839]\n",
      " [ 0.74699648  4.04374595  9.82970475]\n",
      " [ 0.60465727  4.67887416  9.25848784]]\n",
      "sqP = \n",
      "[[1.4399538961078056, 0.0, 0.0], [0.05255109306671703, 0.5521878884084106, 0.0], [0.0487132883892053, 0.000405617349289733, 0.5284431460764409]]\n",
      "m = \n",
      "[-0.13612473  4.84473492  9.80729472]\n",
      "P = \n",
      "[[2.06992618 0.05632437 0.05169043]\n",
      " [0.05632437 0.10209885 0.00514511]\n",
      " [0.05169043 0.00514511 0.07883237]]\n",
      "num = \n",
      "610\n",
      "SENSOR 9 0.829574081247 -1.34777510094\n",
      "\n",
      "num = \n",
      "611\n",
      "SENSOR 3 3.99649915462 -2.37661430213\n",
      "\n",
      "num = \n",
      "612\n",
      "SENSOR 6 4.86215564276 -0.354259931521\n",
      "\n",
      "num = \n",
      "613\n",
      "SENSOR 7 5.17293988297 -1.61785902826\n",
      "\n",
      "num = \n",
      "614\n",
      "ODOMETRY -0.000499775865667 0.0998879891969 -0.000202950445095\n",
      "\n",
      "sqP = \n",
      "[[1.4387238021573463, 0.0, 0.0], [0.03914884428484618, 0.317121772496257, 0.0], [0.03592797001929582, 0.011789061592560383, 0.2782131780284006]]\n",
      "sigma = \n",
      "[[-0.13612473  4.84473492  9.80729472]\n",
      " [ 2.74132287  4.84473492  9.80729472]\n",
      " [-0.05782704  5.47897847  9.80729472]\n",
      " [-0.06426879  4.86831304 10.36372107]\n",
      " [-3.01357233  4.84473492  9.80729472]\n",
      " [-0.21442242  4.21049138  9.80729472]\n",
      " [-0.20798067  4.8211568   9.25086836]]\n",
      "sigmax = \n",
      "[[-0.13539084  4.94381122  9.7937553 ]\n",
      " [ 2.74205677  4.7526299   9.84623178]\n",
      " [-0.05709315  5.57881025  9.80154631]\n",
      " [-0.0635349   4.96810572 10.35732969]\n",
      " [-3.01283844  4.74556005  9.79449729]\n",
      " [-0.21368852  4.30820511  9.78604726]\n",
      " [-0.20724678  4.91900538  9.23025079]]\n",
      "sqP = \n",
      "[[1.507564137916738, 0.0, 0.0], [0.01022814874175676, 0.5550197170320798, 0.0], [0.019158730948621916, 0.006390484675582552, 0.528489433446607]]\n",
      "m = \n",
      "[-0.13438831  4.71118327  9.92226856]\n",
      "P = \n",
      "[[ 2.27240792e+00  1.14185372e-02  2.14451071e-02]\n",
      " [ 1.14185372e-02  1.01131376e-01 -1.41955226e-03]\n",
      " [ 2.14451071e-02 -1.41955226e-03  7.59073908e-02]]\n",
      "num = \n",
      "615\n",
      "SENSOR 9 0.926234276526 -1.50863813963\n",
      "\n",
      "num = \n",
      "616\n",
      "SENSOR 3 4.06561199195 -2.33294732347\n",
      "\n",
      "num = \n",
      "617\n",
      "SENSOR 6 4.52518969077 -0.459458859882\n",
      "\n",
      "num = \n",
      "618\n",
      "SENSOR 7 4.90713478296 -1.6534950895\n",
      "\n",
      "num = \n",
      "619\n",
      "ODOMETRY 0.000159821856273 0.0999413115697 0.000165823443296\n",
      "\n",
      "sqP = \n",
      "[[1.5074508005528116, 0.0, 0.0], [0.007574732884052587, 0.31792137315455044, 0.0], [0.014226074290740232, -0.0048040525228335, 0.27510349088021474]]\n",
      "sigma = \n",
      "[[-0.13438831  4.71118327  9.92226856]\n",
      " [ 2.88051329  4.71118327  9.92226856]\n",
      " [-0.11923884  5.34702601  9.92226856]\n",
      " [-0.10593616  4.70157516 10.47247554]\n",
      " [-3.14928991  4.71118327  9.92226856]\n",
      " [-0.14953777  4.07534052  9.92226856]\n",
      " [-0.16284046  4.72079137  9.37206158]]\n",
      "sigmax = \n",
      "[[-0.13509103  4.81016391  9.90883568]\n",
      " [ 2.87981057  4.61469319  9.94810022]\n",
      " [-0.11994157  5.4461988   9.91033667]\n",
      " [-0.10663889  4.80089789 10.46186393]\n",
      " [ 3.13319267  4.61129863  9.92308734]\n",
      " [-0.1502405   4.17410632  9.90733778]\n",
      " [-0.16354318  4.81934982  9.35581831]]\n",
      "sqP = \n",
      "[[1.4334654647626264, 0.0, 0.0], [-0.07941725052461919, 0.549673941294788, 0.0], [0.013466447377619121, -0.0017308184368011721, 0.5257833279556859]]\n",
      "m = \n",
      "[0.63542653 4.85107925 9.84210674]\n",
      "P = \n",
      "[[ 2.05049583e+00 -8.48490587e-02  1.37937347e-02]\n",
      " [-8.48490587e-02  1.08196884e-01  3.79510316e-04]\n",
      " [ 1.37937347e-02  3.79510316e-04  7.12284909e-02]]\n",
      "num = \n",
      "620\n",
      "SENSOR 9 0.798977474978 -1.61450594503\n",
      "\n",
      "num = \n",
      "621\n",
      "SENSOR 3 4.24005259522 -2.48339290405\n",
      "\n",
      "num = \n",
      "622\n",
      "SENSOR 6 4.37753199658 -0.439993648344\n",
      "\n",
      "num = \n",
      "623\n",
      "SENSOR 7 4.78729857286 -1.52285334952\n",
      "\n",
      "num = \n",
      "624\n",
      "ODOMETRY 0.000239336090803 0.100024339541 -0.000702745140684\n",
      "\n",
      "sqP = \n",
      "[[1.4319552488951073, 0.0, 0.0], [-0.05925398767337326, 0.3235519265577538, 0.0], [0.009632797327024935, 0.0029370616951137026, 0.26669659504053855]]\n",
      "sigma = \n",
      "[[ 0.63542653  4.85107925  9.84210674]\n",
      " [ 3.49933703  4.85107925  9.84210674]\n",
      " [ 0.51691855  5.4981831   9.84210674]\n",
      " [ 0.65469212  4.85695337 10.37549993]\n",
      " [-2.22848397  4.85107925  9.84210674]\n",
      " [ 0.7539345   4.20397539  9.84210674]\n",
      " [ 0.61616093  4.84520512  9.30871355]]\n",
      "sigmax = \n",
      "[[ 0.63575217  4.93150439  9.90143685]\n",
      " [-2.78352264  4.75747091  9.8070961 ]\n",
      " [ 0.5172442   5.5850588   9.89151198]\n",
      " [ 0.65501777  4.93622063 10.43636837]\n",
      " [-2.22815833  4.78999892  9.76300264]\n",
      " [ 0.75426015  4.27682181  9.91052944]\n",
      " [ 0.61648658  4.9267583   9.3664833 ]]\n",
      "sqP = \n",
      "[[1.4399414904627792, 0.0, 0.0], [0.05180244097138684, 0.5558882318412113, 0.0], [0.04847539315287579, -0.00015911681779949427, 0.5214104035003828]]\n",
      "m = \n",
      "[-0.14544414  5.02762628  9.78142836]\n",
      "P = \n",
      "[[2.06988242e+00 5.52133812e-02 5.10576713e-02]\n",
      " [5.52133812e-02 1.05022165e-01 1.13737573e-03]\n",
      " [5.10576713e-02 1.13737573e-03 7.39536949e-02]]\n",
      "num = \n",
      "625\n",
      "SENSOR 9 0.85153318303 -1.74081822727\n",
      "\n",
      "num = \n",
      "626\n",
      "SENSOR 3 4.19445086514 -2.41078939754\n",
      "\n",
      "num = \n",
      "627\n",
      "SENSOR 6 4.3332692815 -0.5738717181\n",
      "\n",
      "num = \n",
      "628\n",
      "SENSOR 7 4.82886107215 -1.58958533264\n",
      "\n",
      "num = \n",
      "629\n",
      "ODOMETRY 0.000302502313308 0.0999098702903 -0.000677343238\n",
      "\n",
      "sqP = \n",
      "[[1.4387085949720044, 0.0, 0.0], [0.038377042727416544, 0.3217908749975979, 0.0], [0.035488542598122834, -0.0006978743236432745, 0.2696178243736251]]\n",
      "sigma = \n",
      "[[-0.14544414  5.02762628  9.78142836]\n",
      " [ 2.73197305  5.02762628  9.78142836]\n",
      " [-0.06869006  5.67120803  9.78142836]\n",
      " [-0.07446706  5.02623053 10.32066401]\n",
      " [-3.02286133  5.02762628  9.78142836]\n",
      " [-0.22219823  4.38404453  9.78142836]\n",
      " [-0.21642123  5.02902203  9.24219271]]\n",
      "sigmax = \n",
      "[[-0.14590755  5.12659799  9.76695533]\n",
      " [ 2.73150964  4.93586718  9.82124214]\n",
      " [-0.06915347  5.77099813  9.77458697]\n",
      " [-0.07493047  5.12597944 10.31324625]\n",
      " [-3.02332474  4.92830898  9.76955645]\n",
      " [-0.22266164  4.48161508  9.75940891]\n",
      " [-0.21688464  5.12671815  9.22073729]]\n",
      "sqP = \n",
      "[[1.5075193714544963, 0.0, 0.0], [0.010004937269915208, 0.5575760627059835, 0.0], [0.018857913501959516, -4.84263674043925e-05, 0.5239996605002739]]\n",
      "m = \n",
      "[-0.14638676  5.08402365  9.75768574]\n",
      "P = \n",
      "[[ 2.27228316  0.01109937  0.0212038 ]\n",
      " [ 0.01109937  0.10074034 -0.00568998]\n",
      " [ 0.0212038  -0.00568998  0.07710245]]\n",
      "num = \n",
      "630\n",
      "SENSOR 9 1.00720101453 -1.80629458243\n",
      "\n",
      "num = \n",
      "631\n",
      "SENSOR 3 4.35581118198 -2.52872962107\n",
      "\n",
      "num = \n",
      "632\n",
      "SENSOR 6 4.26273136747 -0.561343932255\n",
      "\n",
      "num = \n",
      "633\n",
      "SENSOR 7 4.90576859962 -1.57561915161\n",
      "\n",
      "num = \n",
      "634\n",
      "ODOMETRY -0.000113543700968 0.0999631337801 5.71158197463e-05\n",
      "\n",
      "sqP = \n",
      "[[1.5074094193283702, 0.0, 0.0], [0.007363210493640831, 0.3173107740077143, 0.0], [0.014066382690530115, -0.01825830823655393, 0.276715053953995]]\n",
      "sigma = \n",
      "[[-0.14638676  5.08402365  9.75768574]\n",
      " [ 2.86843208  5.08402365  9.75768574]\n",
      " [-0.13166034  5.7186452   9.75768574]\n",
      " [-0.118254    5.04750704 10.31111585]\n",
      " [-3.1612056   5.08402365  9.75768574]\n",
      " [-0.16111318  4.4494021   9.75768574]\n",
      " [-0.17451953  5.12054027  9.20425563]]\n",
      "sigmax = \n",
      "[[-0.1467616   5.18286935  9.74314234]\n",
      " [ 2.86805724  4.98780999  9.78460994]\n",
      " [-0.13203518  5.81769434  9.7445995 ]\n",
      " [-0.11862884  5.14672271 10.29935863]\n",
      " [ 3.12160487  4.98413241  9.75961492]\n",
      " [-0.16148802  4.54802292  9.74168832]\n",
      " [-0.17489437  5.21893776  9.18693755]]\n",
      "sqP = \n",
      "[[1.433464615968317, 0.0, 0.0], [-0.07949119438805345, 0.549572532293548, 0.0], [0.014360005296640622, -0.008573829812213137, 0.5265545883180734]]\n",
      "m = \n",
      "[0.63262173 5.18899706 9.83038285]\n",
      "P = \n",
      "[[ 2.05047037 -0.08464903  0.01606524]\n",
      " [-0.08464903  0.10527138 -0.00885194]\n",
      " [ 0.01606524 -0.00885194  0.07875403]]\n",
      "num = \n",
      "635\n",
      "SENSOR 9 0.926859129886 -2.02052805263\n",
      "\n",
      "num = \n",
      "636\n",
      "SENSOR 3 4.5099200538 -2.5031268995\n",
      "\n",
      "num = \n",
      "637\n",
      "SENSOR 6 4.1118649023 -0.581633953961\n",
      "\n",
      "num = \n",
      "638\n",
      "SENSOR 7 4.76898001883 -1.59231791735\n",
      "\n",
      "num = \n",
      "639\n",
      "ODOMETRY -0.000339024488948 0.100009711461 2.26344713328e-05\n",
      "\n",
      "sqP = \n",
      "[[1.4319463568262334, 0.0, 0.0], [-0.05911466406152063, 0.3190248230868212, 0.0], [0.011219164327493208, -0.025667971873265437, 0.279229862411046]]\n",
      "sigma = \n",
      "[[ 0.63262173  5.18899706  9.83038285]\n",
      " [ 3.49651444  5.18899706  9.83038285]\n",
      " [ 0.5143924   5.82704671  9.83038285]\n",
      " [ 0.65506005  5.13766112 10.38884257]\n",
      " [-2.23127099  5.18899706  9.83038285]\n",
      " [ 0.75085105  4.55094742  9.83038285]\n",
      " [ 0.6101834   5.24033301  9.27192313]]\n",
      "sigmax = \n",
      "[[ 0.6325653   5.26962206  9.88947801]\n",
      " [-2.7867273   5.09526032  9.7956546 ]\n",
      " [ 0.51433597  5.91407938  9.87955543]\n",
      " [ 0.65500363  5.21693993 10.4497318 ]\n",
      " [-2.23132742  5.12766154  9.75144881]\n",
      " [ 0.75079463  4.62403906  9.89857552]\n",
      " [ 0.61012697  5.32226359  9.32919447]]\n",
      "sqP = \n",
      "[[1.4399481075788345, 0.0, 0.0], [0.05195627276573373, 0.5538511181566875, 0.0], [0.04867660085479075, -0.014622881928629065, 0.527778723705896]]\n",
      "m = \n",
      "[-0.14902741  5.34085634  9.79079137]\n",
      "P = \n",
      "[[ 2.0697827   0.05532773  0.05069006]\n",
      " [ 0.05532773  0.10320051 -0.00703017]\n",
      " [ 0.05069006 -0.00703017  0.07797984]]\n",
      "num = \n",
      "640\n",
      "SENSOR 9 1.01426369829 -2.06024600625\n",
      "\n",
      "num = \n",
      "641\n",
      "SENSOR 3 4.50108503541 -2.50066382553\n",
      "\n",
      "num = \n",
      "642\n",
      "SENSOR 6 4.10077035421 -0.552157899173\n",
      "\n",
      "num = \n",
      "643\n",
      "SENSOR 7 4.90578246686 -1.60737195764\n",
      "\n",
      "num = \n",
      "644\n",
      "ODOMETRY -0.000504457264396 0.100013657645 -0.000177576169986\n",
      "\n",
      "sqP = \n",
      "[[1.4386739378328903, 0.0, 0.0], [0.03845744921554724, 0.31893813008174016, 0.0], [0.035233876161840996, -0.026290902704881568, 0.2757665656728203]]\n",
      "sigma = \n",
      "[[-0.14902741  5.34085634  9.79079137]\n",
      " [ 2.72832047  5.34085634  9.79079137]\n",
      " [-0.07211251  5.9787326   9.79079137]\n",
      " [-0.07855966  5.28827454 10.3423245 ]\n",
      " [-3.02637528  5.34085634  9.79079137]\n",
      " [-0.22594231  4.70298008  9.79079137]\n",
      " [-0.21949516  5.39343815  9.23925824]]\n",
      "sigmax = \n",
      "[[-0.1493438   5.4397525   9.77590876]\n",
      " [ 2.72800408  5.24927991  9.83098714]\n",
      " [-0.0724289   6.07847994  9.78355185]\n",
      " [-0.07887605  5.38797313 10.33444205]\n",
      " [-3.02669167  5.24150582  9.77932767]\n",
      " [-0.2262587   4.80044029  9.76835367]\n",
      " [-0.21981155  5.49104099  9.21744934]]\n",
      "sqP = \n",
      "[[1.507482371647891, 0.0, 0.0], [0.00940360014588251, 0.5565390913363459, 0.0], [0.01894907571068366, -0.012934226954414103, 0.5270621603837783]]\n",
      "m = \n",
      "[-0.14783478  5.39169223  9.8244268 ]\n",
      "P = \n",
      "[[ 2.27217484  0.01039525  0.02129551]\n",
      " [ 0.01039525  0.09766801 -0.01240419]\n",
      " [ 0.02129551 -0.01240419  0.08091841]]\n",
      "num = \n",
      "645\n",
      "SENSOR 9 0.887211360034 -2.18963300619\n",
      "\n",
      "num = \n",
      "646\n",
      "SENSOR 3 4.5749440351 -2.48258346425\n",
      "\n",
      "num = \n",
      "647\n",
      "SENSOR 6 3.90635999301 -0.524342529557\n",
      "\n",
      "num = \n",
      "648\n",
      "SENSOR 7 4.99010419492 -1.6897455089\n",
      "\n",
      "num = \n",
      "649\n",
      "ODOMETRY 0.000243302162545 0.100124792442 0.000564451310213\n",
      "\n",
      "sqP = \n",
      "[[1.5073734905028604, 0.0, 0.0], [0.006896268180711197, 0.3124427103056191, 0.0], [0.014127560211861226, -0.0400124986958423, 0.2812788977530819]]\n",
      "sigma = \n",
      "[[-0.14783478  5.39169223  9.8244268 ]\n",
      " [ 2.8669122   5.39169223  9.8244268 ]\n",
      " [-0.13404225  6.01657765  9.8244268 ]\n",
      " [-0.11957966  5.31166723 10.3869846 ]\n",
      " [-3.16258176  5.39169223  9.8244268 ]\n",
      " [-0.16162732  4.76680681  9.8244268 ]\n",
      " [-0.1760899   5.47171723  9.261869  ]]\n",
      "sigmax = \n",
      "[[-0.14851682  5.49060753  9.8096452 ]\n",
      " [ 2.86623016  5.29544159  9.851603  ]\n",
      " [-0.13472428  6.11568741  9.81101086]\n",
      " [-0.1202617   5.41096065 10.37500339]\n",
      " [ 3.11992151  5.29170167  9.82657628]\n",
      " [-0.16230935  4.86550883  9.80828236]\n",
      " [-0.17677194  5.57017544  9.24429881]]\n",
      "sqP = \n",
      "[[1.4334644843315585, 0.0, 0.0], [-0.07989061547813563, 0.5478731503868435, 0.0], [0.014610885905286627, -0.019991600968438043, 0.528672679812924]]\n",
      "m = \n",
      "[0.62767074 5.50351638 9.81051982]\n",
      "P = \n",
      "[[ 2.05040387 -0.084842    0.01692715]\n",
      " [-0.084842    0.10260681 -0.01484019]\n",
      " [ 0.01692715 -0.01484019  0.08060026]]\n",
      "num = \n",
      "650\n",
      "SENSOR 9 0.950336814138 -2.20135217535\n",
      "\n",
      "num = \n",
      "651\n",
      "SENSOR 3 4.64575251122 -2.45474381848\n",
      "\n",
      "num = \n",
      "652\n",
      "SENSOR 6 3.94194574093 -0.493874197364\n",
      "\n",
      "num = \n",
      "653\n",
      "SENSOR 7 5.01810231369 -1.75192836429\n",
      "\n",
      "num = \n",
      "654\n",
      "ODOMETRY -0.000738312989045 0.100017614324 0.000452576665471\n",
      "\n",
      "sqP = \n",
      "[[1.431923138343976, 0.0, 0.0], [-0.059250383610814375, 0.31479548519086975, 0.0], [0.011821273348627032, -0.0449173377378337, 0.28007668270491315]]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "sigma = \n",
      "[[ 0.62767074  5.50351638  9.81051982]\n",
      " [ 3.49151702  5.50351638  9.81051982]\n",
      " [ 0.50916997  6.13310735  9.81051982]\n",
      " [ 0.65131329  5.41368171 10.37067319]\n",
      " [-2.23617554  5.50351638  9.81051982]\n",
      " [ 0.74617151  4.87392541  9.81051982]\n",
      " [ 0.60402819  5.59335106  9.25036645]]\n",
      "sigmax = \n",
      "[[ 0.62847849  5.58454284  9.86933893]\n",
      " [-2.79086054  5.40946764  9.77617148]\n",
      " [ 0.50997773  6.22051938  9.85934719]\n",
      " [ 0.65212104  5.49329502 10.43139135]\n",
      " [-2.23536778  5.44172276  9.73173834]\n",
      " [ 0.74697926  4.94742982  9.87850567]\n",
      " [ 0.60483595  5.67574537  9.30725363]]\n",
      "sqP = \n",
      "[[1.43996074187333, 0.0, 0.0], [0.05224777934192169, 0.5526648181449878, 0.0], [0.048574868930848905, -0.02450009781256341, 0.5278797521930465]]\n",
      "m = \n",
      "[-0.1559765  5.5405842  9.854164 ]\n",
      "P = \n",
      "[[ 2.06973228  0.05517936  0.05066643]\n",
      " [ 0.05517936  0.09810395 -0.01303914]\n",
      " [ 0.05066643 -0.01303914  0.08363303]]\n",
      "num = \n",
      "655\n",
      "SENSOR 9 1.24742332493 -2.3761257833\n",
      "\n",
      "num = \n",
      "656\n",
      "SENSOR 3 4.78533958114 -2.48110189333\n",
      "\n",
      "num = \n",
      "657\n",
      "SENSOR 6 3.83981394901 -0.473736803464\n",
      "\n",
      "num = \n",
      "658\n",
      "SENSOR 7 4.94028734088 -1.67119507241\n",
      "\n",
      "num = \n",
      "659\n",
      "ODOMETRY -0.000158281123923 0.100057519587 8.25146303648e-05\n",
      "\n",
      "sqP = \n",
      "[[1.4386564132916508, 0.0, 0.0], [0.03835478565086773, 0.3108582713671226, 0.0], [0.035217878308921646, -0.046290925421211215, 0.28328409784457426]]\n",
      "sigma = \n",
      "[[-0.1559765   5.5405842   9.854164  ]\n",
      " [ 2.72133633  5.5405842   9.854164  ]\n",
      " [-0.07926693  6.16230075  9.854164  ]\n",
      " [-0.08554074  5.44800235 10.4207322 ]\n",
      " [-3.03328932  5.5405842   9.854164  ]\n",
      " [-0.23268607  4.91886766  9.854164  ]\n",
      " [-0.22641226  5.63316606  9.28759581]]\n",
      "sigmax = \n",
      "[[-0.15626223  5.63937614  9.83855384]\n",
      " [ 2.72105059  5.44929984  9.89503804]\n",
      " [-0.07955266  6.26199843  9.8461706 ]\n",
      " [-0.08582648  5.54764793 10.41211347]\n",
      " [-3.03357506  5.44114465  9.84342634]\n",
      " [-0.23297181  5.0161728   9.83102889]\n",
      " [-0.22669799  5.73061442  9.26507162]]\n",
      "sqP = \n",
      "[[1.5074626575939132, 0.0, 0.0], [0.00880157805162432, 0.5532441164315455, 0.0], [0.019109701886392588, -0.023849403036908363, 0.5307113600267307]]\n",
      "m = \n",
      "[-0.15373629  5.68245546  9.87528013]\n",
      "P = \n",
      "[[ 2.27211669  0.00979804  0.02140668]\n",
      " [ 0.00979804  0.09480185 -0.01595544]\n",
      " [ 0.02140668 -0.01595544  0.08233367]]\n",
      "num = \n",
      "660\n",
      "SENSOR 9 1.25516136996 -2.38310746911\n",
      "\n",
      "num = \n",
      "661\n",
      "SENSOR 3 4.67629555186 -2.57984253808\n",
      "\n",
      "num = \n",
      "662\n",
      "SENSOR 6 3.58479173705 -0.552269803889\n",
      "\n",
      "num = \n",
      "663\n",
      "SENSOR 7 5.04082293108 -1.86475241589\n",
      "\n",
      "num = \n",
      "664\n",
      "ODOMETRY -0.000421693045302 0.100088421092 -0.000583909844015\n",
      "\n",
      "sqP = \n",
      "[[1.5073542032666203, 0.0, 0.0], [0.0065001563675585365, 0.3078304675360489, 0.0], [0.014201490052477991, -0.05213178985127115, 0.28180535917644334]]\n",
      "sigma = \n",
      "[[-0.15373629  5.68245546  9.87528013]\n",
      " [ 2.86097211  5.68245546  9.87528013]\n",
      " [-0.14073598  6.2981164   9.87528013]\n",
      " [-0.12533331  5.57819188 10.43889085]\n",
      " [-3.1684447   5.68245546  9.87528013]\n",
      " [-0.16673661  5.06679453  9.87528013]\n",
      " [-0.18213927  5.78671904  9.31166941]]\n",
      "sigmax = \n",
      "[[-0.15381206  5.78133046  9.85994253]\n",
      " [ 2.86089635  5.5863162   9.90300648]\n",
      " [-0.14081175  6.39718243  9.8612292 ]\n",
      " [-0.12540908  5.67746257 10.4263674 ]\n",
      " [ 3.11466484  5.58243444  9.87798239]\n",
      " [-0.16681237  5.16546178  9.85865846]\n",
      " [-0.18221504  5.88511858  9.29353003]]\n",
      "sqP = \n",
      "[[1.4334642174098888, 0.0, 0.0], [-0.08008843871819814, 0.5462371447174678, 0.0], [0.015085387421406466, -0.026395974798804598, 0.5286754710302012]]\n",
      "m = \n",
      "[0.62082547 5.79964528 9.82731334]\n",
      "P = \n",
      "[[ 2.05030246 -0.08452911  0.01793074]\n",
      " [-0.08452911  0.09781775 -0.01897491]\n",
      " [ 0.01793074 -0.01897491  0.08316682]]\n",
      "num = \n",
      "665\n",
      "SENSOR 9 1.30043780911 -2.42814444723\n",
      "\n",
      "num = \n",
      "666\n",
      "SENSOR 3 4.77942694559 -2.51108430416\n",
      "\n",
      "num = \n",
      "667\n",
      "SENSOR 6 3.7036104257 -0.594306761593\n",
      "\n",
      "num = \n",
      "668\n",
      "SENSOR 7 5.20342066846 -1.82597751664\n",
      "\n",
      "num = \n",
      "669\n",
      "ODOMETRY -0.000492703155817 0.0998767019988 -0.000399546911055\n",
      "\n",
      "sqP = \n",
      "[[1.4318877270259622, 0.0, 0.0], [-0.05903333873312894, 0.3071364762906669, 0.0], [0.01252244559634188, -0.05937316814098442, 0.28193054466072925]]\n",
      "sigma = \n",
      "[[ 0.62082547  5.79964528  9.82731334]\n",
      " [ 3.48460092  5.79964528  9.82731334]\n",
      " [ 0.50275879  6.41391824  9.82731334]\n",
      " [ 0.64587036  5.68089895 10.39117443]\n",
      " [-2.24294998  5.79964528  9.82731334]\n",
      " [ 0.73889215  5.18537233  9.82731334]\n",
      " [ 0.59578058  5.91839162  9.26345225]]\n",
      "sigmax = \n",
      "[[ 0.61981987  5.88108161  9.88550111]\n",
      " [-2.79958999  5.70537311  9.79369118]\n",
      " [ 0.50175319  6.5016417   9.87550343]\n",
      " [ 0.64486476  5.76085258 10.45138331]\n",
      " [-2.24395559  5.73728994  9.74902219]\n",
      " [ 0.73788654  5.25938762  9.89468862]\n",
      " [ 0.59477498  6.00125956  9.31958243]]\n",
      "sqP = \n",
      "[[1.4399611699694246, 0.0, 0.0], [0.05278363812997815, 0.5496478136959124, 0.0], [0.04821563240425235, -0.03223680029076593, 0.5284709057623251]]\n",
      "m = \n",
      "[-0.16031964  5.81416328  9.93755874]\n",
      "P = \n",
      "[[ 2.06965459  0.05539784  0.05032854]\n",
      " [ 0.05539784  0.09182101 -0.01668082]\n",
      " [ 0.05032854 -0.01668082  0.08741839]]\n",
      "num = \n",
      "670\n",
      "SENSOR 9 1.50701523022 -2.40519251821\n",
      "\n",
      "num = \n",
      "671\n",
      "SENSOR 3 4.97415244053 -2.57426809268\n",
      "\n",
      "num = \n",
      "672\n",
      "SENSOR 6 3.73937872599 -0.646973765524\n",
      "\n",
      "num = \n",
      "673\n",
      "SENSOR 7 5.04630156868 -1.81126053621\n",
      "\n",
      "num = \n",
      "674\n",
      "ODOMETRY -0.000686941708413 0.100091264802 -0.000646733220067\n",
      "\n",
      "sqP = \n",
      "[[1.4386294135144904, 0.0, 0.0], [0.0385073735817985, 0.30056312817758674, 0.0], [0.034983668877442496, -0.05998059137202473, 0.2873966917230534]]\n",
      "sigma = \n",
      "[[-0.16031964  5.81416328  9.93755874]\n",
      " [ 2.71693919  5.81416328  9.93755874]\n",
      " [-0.08330489  6.41528954  9.93755874]\n",
      " [-0.0903523   5.6942021  10.51235213]\n",
      " [-3.03757846  5.81416328  9.93755874]\n",
      " [-0.23733438  5.21303702  9.93755874]\n",
      " [-0.23028698  5.93412446  9.36276536]]\n",
      "sigmax = \n",
      "[[-0.16121189  5.91275133  9.92156647]\n",
      " [ 2.71604694  5.72317776  9.97875328]\n",
      " [-0.08419714  6.51481577  9.92919911]\n",
      " [-0.09124455  5.79366695 10.5032913 ]\n",
      " [-3.03847071  5.71482127  9.92723781]\n",
      " [-0.23822663  5.31010241  9.91402865]\n",
      " [-0.23117923  6.03135327  9.3399199 ]]\n",
      "sqP = \n",
      "[[1.5074353267511482, 0.0, 0.0], [0.008308331310268206, 0.5488173256380421, 0.0], [0.01913761037671447, -0.031726151131112726, 0.5325240710549498]]\n",
      "m = \n",
      "[-0.15917257  5.90098354  9.96967842]\n",
      "P = \n",
      "[[ 2.27203803  0.00925952  0.02141611]\n",
      " [ 0.00925952  0.09033196 -0.0191925 ]\n",
      " [ 0.02141611 -0.0191925   0.08399516]]\n",
      "num = \n",
      "675\n",
      "SENSOR 9 1.53949734641 -2.56047560031\n",
      "\n",
      "num = \n",
      "676\n",
      "SENSOR 6 3.54771183278 -0.554710714875\n",
      "\n",
      "num = \n",
      "677\n",
      "ODOMETRY 0.000264360795126 0.0999909746645 -0.000504895193083\n",
      "\n",
      "sqP = \n",
      "[[1.5073281087201107, 0.0, 0.0], [0.006142999605168892, 0.3004899734861696, 0.0], [0.01420799217936057, -0.06416113969690544, 0.28227051815340554]]\n",
      "sigma = \n",
      "[[-0.15917257  5.90098354  9.96967842]\n",
      " [ 2.85548365  5.90098354  9.96967842]\n",
      " [-0.14688657  6.50196348  9.96967842]\n",
      " [-0.13075659  5.77266126 10.53421946]\n",
      " [-3.17382879  5.90098354  9.96967842]\n",
      " [-0.17145857  5.30000359  9.96967842]\n",
      " [-0.18758855  6.02930582  9.40513738]]\n",
      "sigmax = \n",
      "[[-0.16050624  5.9997986   9.95374594]\n",
      " [ 2.85414997  5.80498048  9.99799229]\n",
      " [-0.14822025  6.60096683  9.95496116]\n",
      " [-0.13209026  5.87188911 10.52110096]\n",
      " [ 3.10802285  5.80094651  9.97297314]\n",
      " [-0.17279224  5.39861546  9.95253313]\n",
      " [-0.18892223  6.12762831  9.38640379]]\n",
      "sqP = \n",
      "[[1.4334638619078477, 0.0, 0.0], [-0.08026852639717556, 0.5433801964911009, 0.0], [0.015577785574437734, -0.03288414884076859, 0.528559726608375]]\n",
      "m = \n",
      "[0.61339229 6.02594686 9.94363211]\n",
      "P = \n",
      "[[ 2.05073047 -0.0882028   0.01638825]\n",
      " [-0.0882028   0.12302799  0.00380393]\n",
      " [ 0.01638825  0.00380393  0.15468723]]\n",
      "num = \n",
      "678\n",
      "SENSOR 9 1.51238925612 -2.52278575906\n",
      "\n",
      "num = \n",
      "679\n",
      "SENSOR 6 3.27990661032 -0.58279346945\n",
      "\n",
      "num = \n",
      "680\n",
      "ODOMETRY -1.55641718837e-05 0.100163521473 -0.000133745408038\n",
      "\n",
      "sqP = \n",
      "[[1.432037173166975, 0.0, 0.0], [-0.061592537793904195, 0.34530327812888534, 0.0], [0.011444013374701238, 0.013057503301936729, 0.39291954702469445]]\n",
      "sigma = \n",
      "[[ 0.61339229  6.02594686  9.94363211]\n",
      " [ 3.47746663  6.02594686  9.94363211]\n",
      " [ 0.49020721  6.71655342  9.94363211]\n",
      " [ 0.63628032  6.05206187 10.72947121]\n",
      " [-2.25068206  6.02594686  9.94363211]\n",
      " [ 0.73657736  5.33534031  9.94363211]\n",
      " [ 0.59050426  5.99983186  9.15779302]]\n",
      "sigmax = \n",
      "[[ 0.61315175  6.10769426 10.00121299]\n",
      " [-2.80595921  5.93155184  9.91065069]\n",
      " [ 0.48996668  6.80475654  9.99073205]\n",
      " [ 0.63603978  6.13247006 10.78890788]\n",
      " [-2.25092259  5.96310268  9.86585806]\n",
      " [ 0.73633683  5.40939307 10.01082127]\n",
      " [ 0.59026373  6.08287564  9.21348794]]\n",
      "sqP = \n",
      "[[1.4400449764065724, 0.0, 0.0], [0.051448750830976435, 0.5692698723796596, 0.0], [0.0484097776457372, 0.006920880374773699, 0.596042834645257]]\n",
      "m = \n",
      "[-0.17467581  6.18197191  9.81344499]\n",
      "P = \n",
      "[[2.07107206 0.05636424 0.05654502]\n",
      " [0.05636424 0.12322797 0.01474079]\n",
      " [0.05654502 0.01474079 0.184047  ]]\n",
      "num = \n",
      "681\n",
      "SENSOR 9 1.5098952529 -2.57185137154\n",
      "\n",
      "num = \n",
      "682\n",
      "SENSOR 6 3.39256354505 -0.708652206512\n",
      "\n",
      "num = \n",
      "683\n",
      "ODOMETRY -0.00944281048405 0.0998363016919 -0.0197064530302\n",
      "\n",
      "sqP = \n",
      "[[1.4391219770534298, 0.0, 0.0], [0.039165714204670646, 0.3488467020816758, 0.0], [0.039291332446529756, 0.03784446832034923, 0.4255243639216667]]\n",
      "sigma = \n",
      "[[-0.17467581  6.18197191  9.81344499]\n",
      " [ 2.70356814  6.18197191  9.81344499]\n",
      " [-0.09634438  6.87966531  9.81344499]\n",
      " [-0.09609315  6.25766085 10.66449372]\n",
      " [-3.05291977  6.18197191  9.81344499]\n",
      " [-0.25300724  5.48427851  9.81344499]\n",
      " [-0.25325848  6.10628297  8.96239627]]\n",
      "sigmax = \n",
      "[[-0.17482512  6.28061097  9.79603615]\n",
      " [ 2.70341883  6.09126535  9.8559309 ]\n",
      " [-0.09649369  6.97936417  9.80380817]\n",
      " [-0.09624246  6.35736212 10.65488195]\n",
      " [-3.05306908  6.08220178  9.80457639]\n",
      " [-0.25315655  5.58125283  9.78837089]\n",
      " [-0.25340779  6.203251    8.9372978 ]]\n",
      "sqP = \n",
      "[[1.5080284568975342, 0.0, 0.0], [0.012243941697467118, 0.5749282275359254, 0.0], [0.023540035452739874, 0.028511734813946785, 0.6192032806600852]]\n",
      "m = \n",
      "[-0.17722756  6.17718283  9.77126493]\n",
      "P = \n",
      "[[2.27382292 0.01382273 0.02934129]\n",
      " [0.01382273 0.11103233 0.01766608]\n",
      " [0.02934129 0.01766608 0.21794906]]\n",
      "num = \n",
      "684\n",
      "SENSOR 9 1.7405212542 -2.61856151363\n",
      "\n",
      "num = \n",
      "685\n",
      "SENSOR 6 3.4449354883 -0.653656313882\n",
      "\n",
      "num = \n",
      "686\n",
      "ODOMETRY -0.00982171572374 0.0999283940399 -0.0201816524328\n",
      "\n",
      "sqP = \n",
      "[[1.5079200644520507, 0.0, 0.0], [0.009166754982272228, 0.3330890348568827, 0.0], [0.01945811782671336, 0.05250161753528623, 0.4634803323104579]]\n",
      "sigma = \n",
      "[[-0.17722756  6.17718283  9.77126493]\n",
      " [ 2.83861257  6.17718283  9.77126493]\n",
      " [-0.15889405  6.8433609   9.77126493]\n",
      " [-0.13831132  6.28218607 10.6982256 ]\n",
      " [-3.19306769  6.17718283  9.77126493]\n",
      " [-0.19556107  5.51100476  9.77126493]\n",
      " [-0.21614379  6.0721796   8.84430427]]\n",
      "sigmax = \n",
      "[[-0.20637682  6.27528474  9.7527365 ]\n",
      " [ 2.80946331  6.08217944  9.80195113]\n",
      " [-0.18804331  6.94178599  9.75453806]\n",
      " [-0.16746059  6.38093457 10.68352798]\n",
      " [ 3.06096836  6.07753172  9.77734298]\n",
      " [-0.22471033  5.60875051  9.75094116]\n",
      " [-0.24529306  6.16948635  8.82197307]]\n",
      "sqP = \n",
      "[[1.4335155727499873, 0.0, 0.0], [-0.07749411136748963, 0.561258205081743, 0.0], [0.021217601880707142, 0.04489978576280705, 0.6437503528564286]]\n",
      "m = \n",
      "[0.58030038 6.25313306 9.87865156]\n",
      "P = \n",
      "[[ 2.05073928 -0.08255394  0.02366002]\n",
      " [-0.08255394  0.11261041  0.0179222 ]\n",
      " [ 0.02366002  0.0179222   0.23999125]]\n",
      "num = \n",
      "687\n",
      "SENSOR 9 1.65301995925 -2.56952419519\n",
      "\n",
      "num = \n",
      "688\n",
      "SENSOR 6 3.02584430907 -0.625348664803\n",
      "\n",
      "num = \n",
      "689\n",
      "ODOMETRY -0.011467407889 0.100278553593 -0.0200899025046\n",
      "\n",
      "sqP = \n",
      "[[1.4320402492772928, 0.0, 0.0], [-0.05764778183340742, 0.33058605304761646, 0.0], [0.016521893875097244, 0.05709452561839931, 0.4862699760700172]]\n",
      "sigma = \n",
      "[[ 0.58030038  6.25313306  9.87865156]\n",
      " [ 3.44438088  6.25313306  9.87865156]\n",
      " [ 0.46500482  6.91430517  9.87865156]\n",
      " [ 0.61334417  6.36732211 10.85119151]\n",
      " [-2.28378011  6.25313306  9.87865156]\n",
      " [ 0.69559595  5.59196095  9.87865156]\n",
      " [ 0.5472566   6.13894401  8.90611161]]\n",
      "sigmax = \n",
      "[[ 0.55029702  6.33723705  9.93261639]\n",
      " [-2.86880779  6.15746248  9.84979288]\n",
      " [ 0.43500145  7.0040589   9.92258276]\n",
      " [ 0.5833408   6.44959731 10.90790549]\n",
      " [-2.31378348  6.1870313   9.80370997]\n",
      " [ 0.66559258  5.66929843  9.94193345]\n",
      " [ 0.51725323  6.22478496  8.95726837]]\n",
      "sqP = \n",
      "[[1.4399306629070225, 0.0, 0.0], [0.05540987282615498, 0.5626579281452716, 0.0], [0.04793344854652544, 0.04677720271372665, 0.6599835296022122]]\n",
      "m = \n",
      "[-0.23683522  6.41051112  9.70761897]\n",
      "P = \n",
      "[[2.07034233 0.05877487 0.05395884]\n",
      " [0.05877487 0.11331691 0.0184398 ]\n",
      " [0.05395884 0.0184398  0.22643426]]\n",
      "num = \n",
      "690\n",
      "SENSOR 9 1.84416645507 -2.56385985305\n",
      "\n",
      "num = \n",
      "691\n",
      "SENSOR 6 3.02960198478 -0.517486552302\n",
      "\n",
      "num = \n",
      "692\n",
      "ODOMETRY -0.00949998918607 0.100016536484 -0.0192452653175\n",
      "\n",
      "sqP = \n",
      "[[1.4388684198410502, 0.0, 0.0], [0.04084798297092115, 0.3341382216131388, 0.0], [0.03750088422098596, 0.05060171748494222, 0.47166451453202535]]\n",
      "sigma = \n",
      "[[-0.23683522  6.41051112  9.70761897]\n",
      " [ 2.64090162  6.41051112  9.70761897]\n",
      " [-0.15513925  7.07878757  9.70761897]\n",
      " [-0.16183345  6.51171456 10.650948  ]\n",
      " [-3.11457206  6.41051112  9.70761897]\n",
      " [-0.31853118  5.74223468  9.70761897]\n",
      " [-0.31183698  6.30930769  8.76428995]]\n",
      "sigmax = \n",
      "[[-0.26839253  6.50771423  9.68297462]\n",
      " [ 2.60934431  6.32309944  9.75676147]\n",
      " [-0.18669656  7.17767758  9.69098908]\n",
      " [-0.19339076  6.61049104 10.6336565 ]\n",
      " [ 3.13705594  6.3102447   9.70605939]\n",
      " [-0.35008849  5.83710249  9.67512454]\n",
      " [-0.34339429  6.40439089  8.7324313 ]]\n",
      "sqP = \n",
      "[[1.4391075956535098, 0.0, 0.0], [-0.06755409030997352, 0.563251200094842, 0.0], [0.031141149397233184, 0.046573302407589126, 0.6506862524415691]]\n",
      "m = \n",
      "[0.50724497 6.5403035  9.69324494]\n",
      "P = \n",
      "[[ 2.06742177 -0.07150482  0.03551383]\n",
      " [-0.07150482  0.10580405  0.02067818]\n",
      " [ 0.03551383  0.02067818  0.25440369]]\n",
      "num = \n",
      "693\n",
      "SENSOR 9 1.83946050295 -2.63003003289\n",
      "\n",
      "num = \n",
      "694\n",
      "SENSOR 6 3.00582309666 -0.577468671279\n",
      "\n",
      "num = \n",
      "695\n",
      "ODOMETRY -0.00998770181841 0.100041220527 -0.0210200884984\n",
      "\n",
      "sqP = \n",
      "[[1.437853180258012, 0.0, 0.0], [-0.04973026759519052, 0.32145131674256516, 0.0], [0.02469920634899034, 0.06814865468330972, 0.4991486749527568]]\n",
      "sigma = \n",
      "[[ 0.50724497  6.5403035   9.69324494]\n",
      " [ 3.38295133  6.5403035   9.69324494]\n",
      " [ 0.40778444  7.18320613  9.69324494]\n",
      " [ 0.55664339  6.67660081 10.69154229]\n",
      " [-2.36846139  6.5403035   9.69324494]\n",
      " [ 0.60670551  5.89740086  9.69324494]\n",
      " [ 0.45784656  6.40400619  8.69494759]]\n",
      "sigmax = \n",
      "[[ 0.47849972  6.62818417  9.74099737]\n",
      " [-2.92897923  6.44296331  9.67026246]\n",
      " [ 0.37903918  7.27539414  9.73203512]\n",
      " [ 0.52789813  6.76201634 10.74357587]\n",
      " [-2.39720664  6.4680586   9.62407861]\n",
      " [ 0.57796025  5.98010557  9.74948763]\n",
      " [ 0.42910131  6.4941376   8.73830237]]\n",
      "sqP = \n",
      "[[1.4392038965083238, 0.0, 0.0], [0.06113865734795048, 0.5578763276277703, 0.0], [0.04604975374994878, 0.05790499403479228, 0.6690636003709266]]\n",
      "m = \n",
      "[-0.30817347  6.58759042  9.69097657]\n",
      "P = \n",
      "[[2.06797554 0.06426284 0.05394141]\n",
      " [0.06426284 0.10339744 0.02598226]\n",
      " [0.05394141 0.02598226 0.26215619]]\n",
      "num = \n",
      "696\n",
      "SENSOR 9 2.1231956515 -2.5712329806\n",
      "\n",
      "num = \n",
      "697\n",
      "SENSOR 6 2.72060090136 -0.611342981081\n",
      "\n",
      "num = \n",
      "698\n",
      "ODOMETRY -0.0104435233728 0.100064843828 -0.0209923840156\n",
      "\n",
      "sqP = \n",
      "[[1.4380457367397896, 0.0, 0.0], [0.044687615923784896, 0.31843438713236416, 0.0], [0.03751022132737532, 0.0763297371320766, 0.504898941513367]]\n",
      "sigma = \n",
      "[[-0.30817347  6.58759042  9.69097657]\n",
      " [ 2.567918    6.58759042  9.69097657]\n",
      " [-0.21879824  7.22445919  9.69097657]\n",
      " [-0.23315303  6.74024989 10.70077445]\n",
      " [-3.18426494  6.58759042  9.69097657]\n",
      " [-0.3975487   5.95072164  9.69097657]\n",
      " [-0.38319391  6.43493094  8.68117869]]\n",
      "sigmax = \n",
      "[[-0.33918126  6.6826108   9.65968162]\n",
      " [ 2.53691021  6.50411102  9.74610765]\n",
      " [-0.24980603  7.32189359  9.6682877 ]\n",
      " [-0.26416082  6.83734856 10.67668931]\n",
      " [ 3.06791257  6.48768788  9.69624231]\n",
      " [-0.42855649  6.04256949  9.65132536]\n",
      " [-0.4142017   6.5273385   8.64284998]]\n",
      "sqP = \n",
      "[[1.4392975870490488, 0.0, 0.0], [-0.06475418318537568, 0.5573541748542203, 0.0], [0.03737086673300774, 0.07371698398571896, 0.6727417074957124]]\n",
      "m = \n",
      "[0.42140322 6.83307746 9.64742288]\n",
      "P = \n",
      "[[ 2.06806823 -0.06940936  0.04287374]\n",
      " [-0.06940936  0.10099452  0.02324041]\n",
      " [ 0.04287374  0.02324041  0.2733237 ]]\n",
      "num = \n",
      "699\n",
      "SENSOR 9 2.2533253491 -2.51358315181\n",
      "\n",
      "num = \n",
      "700\n",
      "SENSOR 6 2.76502567229 -0.610392844017\n",
      "\n",
      "num = \n",
      "701\n",
      "ODOMETRY -0.0101773588174 0.100165429114 -0.0197769867168\n",
      "\n",
      "sqP = \n",
      "[[1.4380779637659675, 0.0, 0.0], [-0.048265365591529545, 0.3141098094378568, 0.0], [0.029813223525971252, 0.07856920341737181, 0.5160055745063291]]\n",
      "sigma = \n",
      "[[ 0.42140322  6.83307746  9.64742288]\n",
      " [ 3.29755915  6.83307746  9.64742288]\n",
      " [ 0.32487249  7.46129708  9.64742288]\n",
      " [ 0.48102967  6.99021587 10.67943403]\n",
      " [-2.4547527   6.83307746  9.64742288]\n",
      " [ 0.51793395  6.20485784  9.64742288]\n",
      " [ 0.36177678  6.67593906  8.61541173]]\n",
      "sigmax = \n",
      "[[ 0.38996732  6.92481069  9.68739772]\n",
      " [-3.01706206  6.73407028  9.63291249]\n",
      " [ 0.29343658  7.55645606  9.67837028]\n",
      " [ 0.44959376  7.07940393 10.72480431]\n",
      " [-2.48618861  6.75504354  9.5847836 ]\n",
      " [ 0.48649805  6.2923112   9.69605294]\n",
      " [ 0.33034087  6.76989141  8.64984904]]\n",
      "sqP = \n",
      "[[1.4392301285416542, 0.0, 0.0], [0.06518175897399342, 0.5547287371543602, 0.0], [0.0418979770831799, 0.06957794552560125, 0.6810375261624977]]\n",
      "m = \n",
      "[-0.38327075  6.96000743  9.72246655]\n",
      "P = \n",
      "[[2.06786072 0.0685424  0.04912339]\n",
      " [0.0685424  0.10173154 0.02990955]\n",
      " [0.04912339 0.02990955 0.27177154]]\n",
      "num = \n",
      "702\n",
      "SENSOR 9 2.27395359652 -2.62138399446\n",
      "\n",
      "num = \n",
      "703\n",
      "SENSOR 6 2.81928646279 -0.499702125893\n",
      "\n",
      "num = \n",
      "704\n",
      "ODOMETRY -0.00963806206216 0.100194140395 -0.0202377586004\n",
      "\n",
      "sqP = \n",
      "[[1.4380058140402414, 0.0, 0.0], [0.04766489849009114, 0.31537215245600614, 0.0], [0.034160773353011084, 0.08967590884954676, 0.512408837363051]]\n",
      "sigma = \n",
      "[[-0.38327075  6.96000743  9.72246655]\n",
      " [ 2.49274088  6.96000743  9.72246655]\n",
      " [-0.28794095  7.59075173  9.72246655]\n",
      " [-0.3149492   7.13935925 10.74728422]\n",
      " [-3.25928237  6.96000743  9.72246655]\n",
      " [-0.47860054  6.32926312  9.72246655]\n",
      " [-0.45159229  6.78065561  8.69764887]]\n",
      "sigmax = \n",
      "[[-0.41322509  7.05251948  9.68406559]\n",
      " [ 2.46278654  6.88081793  9.78380281]\n",
      " [-0.3178953   7.68649895  9.69304575]\n",
      " [-0.34490355  7.23427704 10.71528851]\n",
      " [ 2.99394859  6.86065974  9.73523954]\n",
      " [-0.50855489  6.41769992  9.67543415]\n",
      " [-0.48154664  6.87033026  8.65302186]]\n",
      "sqP = \n",
      "[[1.439313394110555, 0.0, 0.0], [-0.06210914193204543, 0.5578920031705705, 0.0], [0.04210689144824142, 0.08678539847630679, 0.6765619913491482]]\n",
      "m = \n",
      "[0.37905533 7.00121796 9.78465012]\n",
      "P = \n",
      "[[ 2.06814515 -0.0672445   0.04737951]\n",
      " [-0.0672445   0.10100067  0.02377527]\n",
      " [ 0.04737951  0.02377527  0.26214988]]\n",
      "num = \n",
      "705\n",
      "SENSOR 9 2.19331599608 -2.58825731994\n",
      "\n",
      "num = \n",
      "706\n",
      "SENSOR 6 2.6367644419 -0.585514977039\n",
      "\n",
      "num = \n",
      "707\n",
      "ODOMETRY -0.00969850517913 0.100084649032 -0.0200844906424\n",
      "\n",
      "sqP = \n",
      "[[1.4381047062369967, 0.0, 0.0], [-0.04675910963066428, 0.3143473429012537, 0.0], [0.03294580173136222, 0.08053442473891287, 0.504557884973421]]\n",
      "sigma = \n",
      "[[ 0.37905533  7.00121796  9.78465012]\n",
      " [ 3.25526474  7.00121796  9.78465012]\n",
      " [ 0.28553711  7.62991265  9.78465012]\n",
      " [ 0.44494693  7.16228681 10.79376589]\n",
      " [-2.49715409  7.00121796  9.78465012]\n",
      " [ 0.47257355  6.37252328  9.78465012]\n",
      " [ 0.31316372  6.84014911  8.77553435]]\n",
      "sigmax = \n",
      "[[ 0.34917951  7.0946528   9.82082742]\n",
      " [-3.05779639  6.90156554  9.77424531]\n",
      " [ 0.25566129  7.72631751  9.81194421]\n",
      " [ 0.41507111  7.25313683 10.8360168 ]\n",
      " [-2.52702991  6.92054264  9.7252333 ]\n",
      " [ 0.44269773  6.46217153  9.82939447]\n",
      " [ 0.2832879   6.93576324  8.80548103]]\n",
      "sqP = \n",
      "[[1.439246237290795, 0.0, 0.0], [0.06704520662335134, 0.5549848978292861, 0.0], [0.03969468790277953, 0.06958923467267716, 0.6725983168757749]]\n",
      "m = \n",
      "[-0.44326486  7.02985073  9.7002156 ]\n",
      "P = \n",
      "[[2.06774653 0.07073213 0.04485657]\n",
      " [0.07073213 0.10573324 0.02443991]\n",
      " [0.04485657 0.02443991 0.238694  ]]\n",
      "num = \n",
      "708\n",
      "SENSOR 9 2.38239873462 -2.51826314964\n",
      "\n",
      "num = \n",
      "709\n",
      "SENSOR 6 2.55584502849 -0.617585417568\n",
      "\n",
      "num = \n",
      "710\n",
      "ODOMETRY -0.009760488778 0.100025282561 -0.0212037030791\n",
      "\n",
      "sqP = \n",
      "[[1.4379661102342374, 0.0, 0.0], [0.04918901359574482, 0.3214244563083185, 0.0], [0.031194454246935052, 0.0712624316730622, 0.4823303577217353]]\n",
      "sigma = \n",
      "[[-0.44326486  7.02985073  9.7002156 ]\n",
      " [ 2.43266736  7.02985073  9.7002156 ]\n",
      " [-0.34488684  7.67269964  9.7002156 ]\n",
      " [-0.38087596  7.17237559 10.66487632]\n",
      " [-3.31919708  7.02985073  9.7002156 ]\n",
      " [-0.54164289  6.38700181  9.7002156 ]\n",
      " [-0.50565377  6.88732586  8.73555489]]\n",
      "sigmax = \n",
      "[[-0.47304786  7.11984226  9.65641536]\n",
      " [ 2.40288436  6.95451576  9.76610627]\n",
      " [-0.37466983  7.76655808  9.66546606]\n",
      " [-0.41065895  7.26492291 10.62677212]\n",
      " [ 2.93420523  6.93151656  9.71885233]\n",
      " [-0.57142589  6.47225618  9.64778823]\n",
      " [-0.53543677  6.97441143  8.68622903]]\n",
      "sqP = \n",
      "[[1.439300991686622, 0.0, 0.0], [-0.06018279529095027, 0.5587809771230698, 0.0], [0.044757336156745105, 0.0654622522593676, 0.6562110244630351]]\n",
      "m = \n",
      "[0.30352192 7.15456953 9.6740729 ]\n",
      "P = \n",
      "[[ 2.06810347 -0.06447055  0.05050956]\n",
      " [-0.06447055  0.10328497  0.02445774]\n",
      " [ 0.05050956  0.02445774  0.24785632]]\n",
      "num = \n",
      "711\n",
      "SENSOR 9 2.55229313835 -2.55940451124\n",
      "\n",
      "num = \n",
      "712\n",
      "SENSOR 6 2.63729703758 -0.564546146269\n",
      "\n",
      "num = \n",
      "713\n",
      "ODOMETRY -0.00901024455226 0.0999822225335 -0.0203196475783\n",
      "\n",
      "sqP = \n",
      "[[1.4380902160842914, 0.0, 0.0], [-0.04483067138448782, 0.31823761840932835, 0.0], [0.035122666808645085, 0.0818014890271827, 0.48982775567457515]]\n",
      "sigma = \n",
      "[[ 0.30352192  7.15456953  9.6740729 ]\n",
      " [ 3.17970235  7.15456953  9.6740729 ]\n",
      " [ 0.21386058  7.79104477  9.6740729 ]\n",
      " [ 0.37376726  7.31817251 10.65372841]\n",
      " [-2.57265851  7.15456953  9.6740729 ]\n",
      " [ 0.39318326  6.51809429  9.6740729 ]\n",
      " [ 0.23327659  6.99096655  8.69441739]]\n",
      "sigmax = \n",
      "[[ 0.27255773  7.25030988  9.70303568]\n",
      " [-3.13444714  7.05458444  9.67123764]\n",
      " [ 0.18289639  7.8889939   9.69434663]\n",
      " [ 0.34280306  7.41164392 10.68933955]\n",
      " [-2.6036227   7.06977872  9.6210109 ]\n",
      " [ 0.36221907  6.6108567   9.71149205]\n",
      " [ 0.2023124   7.08850361  8.71658896]]\n",
      "sqP = \n",
      "[[1.43923768983609, 0.0, 0.0], [0.06928069480487617, 0.557193405131103, 0.0], [0.034278281240274384, 0.06829429675256894, 0.6619493554109765]]\n",
      "m = \n",
      "[-0.50137147  7.26158704  9.796565  ]\n",
      "P = \n",
      "[[2.06775141 0.07320485 0.04014651]\n",
      " [0.07320485 0.1055706  0.02929986]\n",
      " [0.04014651 0.02929986 0.2436744 ]]\n",
      "num = \n",
      "714\n",
      "SENSOR 9 2.542229302 -2.53541280471\n",
      "\n",
      "num = \n",
      "715\n",
      "SENSOR 6 2.27227869215 -0.573875753025\n",
      "\n",
      "num = \n",
      "716\n",
      "ODOMETRY -0.00961564138041 0.0999414405798 -0.0204022645065\n",
      "\n",
      "sqP = \n",
      "[[1.4379678061440402, 0.0, 0.0], [0.05090854280067043, 0.32090328425674175, 0.0], [0.027918918200534656, 0.08687525058861742, 0.4851264058479524]]\n",
      "sigma = \n",
      "[[-0.50137147  7.26158704  9.796565  ]\n",
      " [ 2.37456414  7.26158704  9.796565  ]\n",
      " [-0.39955438  7.90339361  9.796565  ]\n",
      " [-0.44553363  7.43533754 10.76681781]\n",
      " [-3.37730708  7.26158704  9.796565  ]\n",
      " [-0.60318855  6.61978047  9.796565  ]\n",
      " [-0.5572093   7.08783654  8.82631219]]\n",
      "sigmax = \n",
      "[[-0.53070136  7.34882734  9.74772265]\n",
      " [ 2.34523425  7.19023035  9.86659833]\n",
      " [-0.42888427  7.9951465   9.75684282]\n",
      " [-0.47486352  7.52516771 10.72292037]\n",
      " [ 2.87654834  7.16458388  9.82078962]\n",
      " [-0.63251845  6.70160457  9.73910839]\n",
      " [-0.5865392   7.17221504  8.77267718]]\n",
      "sqP = \n",
      "[[1.4392933637122507, 0.0, 0.0], [-0.05751745124211368, 0.5608868902799407, 0.0], [0.04779002378616238, 0.07880343297282778, 0.6565786331468095]]\n",
      "m = \n",
      "[0.23326518 7.39375741 9.62590753]\n",
      "P = \n",
      "[[ 2.06816252 -0.06278311  0.05289818]\n",
      " [-0.06278311  0.10591044  0.02102862]\n",
      " [ 0.05289818  0.02102862  0.228295  ]]\n",
      "num = \n",
      "717\n",
      "SENSOR 9 2.70341312756 -2.48331655508\n",
      "\n",
      "num = \n",
      "718\n",
      "SENSOR 6 2.28488250206 -0.552514217678\n",
      "\n",
      "num = \n",
      "719\n",
      "ODOMETRY -0.00990457116932 0.100026465638 -0.0207767178837\n",
      "\n",
      "sqP = \n",
      "[[1.4381107462347977, 0.0, 0.0], [-0.043656661323330605, 0.32249733845020645, 0.0], [0.03678310730647127, 0.0701849045388165, 0.4711858262657813]]\n",
      "sigma = \n",
      "[[ 0.23326518  7.39375741  9.62590753]\n",
      " [ 3.10948667  7.39375741  9.62590753]\n",
      " [ 0.14595185  8.03875209  9.62590753]\n",
      " [ 0.30683139  7.53412722 10.56827918]\n",
      " [-2.64295632  7.39375741  9.62590753]\n",
      " [ 0.3205785   6.74876274  9.62590753]\n",
      " [ 0.15969896  7.25338761  8.68353588]]\n",
      "sigmax = \n",
      "[[ 0.20324727  7.49120977  9.64807351]\n",
      " [ 3.07946876  7.29390295  9.63007604]\n",
      " [ 0.11593395  8.13776614  9.63949099]\n",
      " [ 0.27681349  7.62968679 10.59754795]\n",
      " [-2.67297422  7.30552978  9.57895888]\n",
      " [ 0.29056059  6.84391093  9.65648716]\n",
      " [ 0.12968106  7.35220557  8.69847917]]\n",
      "sqP = \n",
      "[[1.507123558138823, 0.0, 0.0], [-0.010450279694833222, 0.5623172731292625, 0.0], [0.023904440764223947, 0.05927868344063019, 0.6495034993775439]]\n",
      "m = \n",
      "[0.20167418 7.51456924 9.63075026]\n",
      "P = \n",
      "[[ 2.27106563 -0.01199794  0.0285967 ]\n",
      " [-0.01199794  0.1046128   0.02834805]\n",
      " [ 0.0285967   0.02834805  0.23066259]]\n",
      "num = \n",
      "720\n",
      "SENSOR 9 2.56436735907 -2.50931464148\n",
      "\n",
      "num = \n",
      "721\n",
      "SENSOR 6 2.08017880027 -0.487502087503\n",
      "\n",
      "num = \n",
      "722\n",
      "ODOMETRY -0.00998320897741 0.0999813680912 -0.0199150821834\n",
      "\n",
      "sqP = \n",
      "[[1.5070055161862383, 0.0, 0.0], [-0.00796144297368571, 0.3233410252273419, 0.0], [0.018975844871274128, 0.08813951385385228, 0.4717350227665849]]\n",
      "sigma = \n",
      "[[ 0.20167418  7.51456924  9.63075026]\n",
      " [ 3.21568521  7.51456924  9.63075026]\n",
      " [ 0.18575129  8.16125129  9.63075026]\n",
      " [ 0.23962587  7.69084827 10.5742203 ]\n",
      " [-2.81233685  7.51456924  9.63075026]\n",
      " [ 0.21759706  6.86788719  9.63075026]\n",
      " [ 0.16372249  7.33829022  8.68728021]]\n",
      "sigmax = \n",
      "[[ 0.17099289  7.61276207  9.64981494]\n",
      " [-3.09818139  7.41474877  9.62433417]\n",
      " [ 0.15507     8.25973523  9.64824907]\n",
      " [ 0.20894458  7.78824703 10.59699694]\n",
      " [-2.84301814  7.41960019  9.59934688]\n",
      " [ 0.18691577  6.96576402  9.65137597]\n",
      " [ 0.1330412   7.4371357   8.70260547]]\n",
      "sqP = \n",
      "[[1.4335423118921764, 0.0, 0.0], [0.07977569574305796, 0.5596074854470087, 0.0], [0.021597031664040738, 0.07323033962037917, 0.6471288653413324]]\n",
      "m = \n",
      "[-0.62237285  7.52878473  9.46808387]\n",
      "P = \n",
      "[[2.05068151 0.08440134 0.02568314]\n",
      " [0.08440134 0.11092534 0.02975186]\n",
      " [0.02568314 0.02975186 0.22562025]]\n",
      "num = \n",
      "723\n",
      "SENSOR 9 2.658209228 -2.52967300837\n",
      "\n",
      "num = \n",
      "724\n",
      "SENSOR 6 2.08742861878 -0.468280843216\n",
      "\n",
      "num = \n",
      "725\n",
      "ODOMETRY -0.0107252267638 0.10021159563 -0.0192940861751\n",
      "\n",
      "sqP = \n",
      "[[1.43202007987825, 0.0, 0.0], [0.05893865353706918, 0.32779807811321987, 0.0], [0.017934903637454612, 0.08753803279733748, 0.4665143947858992]]\n",
      "sigma = \n",
      "[[-0.62237285  7.52878473  9.46808387]\n",
      " [ 2.24166731  7.52878473  9.46808387]\n",
      " [-0.50449554  8.18438088  9.46808387]\n",
      " [-0.58650304  7.70386079 10.40111266]\n",
      " [-3.48641301  7.52878473  9.46808387]\n",
      " [-0.74025015  6.87318857  9.46808387]\n",
      " [-0.65824265  7.35370866  8.53505508]]\n",
      "sigmax = \n",
      "[[-0.65227114  7.60943342  9.4089902 ]\n",
      " [ 2.21176902  7.46741437  9.54701392]\n",
      " [-0.53439383  8.2714196   9.41888493]\n",
      " [-0.61640133  7.78657683 10.34494923]\n",
      " [ 2.76687401  7.43503072  9.50281802]\n",
      " [-0.77014844  6.94632792  9.39991563]\n",
      " [-0.68814094  7.43218625  8.47310719]]\n",
      "sqP = \n",
      "[[1.4400017699856387, 0.0, 0.0], [-0.050752417869130574, 0.5654752336837133, 0.0], [0.05231724946748922, 0.07488536238411067, 0.6428841313134072]]\n",
      "m = \n",
      "[0.12969096 7.56986467 9.40080122]\n",
      "P = \n",
      "[[ 2.07052312 -0.05400458  0.06042298]\n",
      " [-0.05400458  0.10549698  0.03552128]\n",
      " [ 0.06042298  0.03552128  0.25516811]]\n",
      "num = \n",
      "726\n",
      "SENSOR 9 2.79062572373 -2.51801590567\n",
      "\n",
      "num = \n",
      "727\n",
      "SENSOR 6 1.98134535694 -0.51239911617\n",
      "\n",
      "num = \n",
      "728\n",
      "ODOMETRY -0.00931898482752 0.0998313946401 -0.0195045943639\n",
      "\n",
      "sqP = \n",
      "[[1.4389312429159344, 0.0, 0.0], [-0.037531035277477746, 0.3226273475884919, 0.0], [0.041991565231760376, 0.11498488308090252, 0.49008498754114277]]\n",
      "sigma = \n",
      "[[ 0.12969096  7.56986467  9.40080122]\n",
      " [ 3.00755345  7.56986467  9.40080122]\n",
      " [ 0.05462889  8.21511936  9.40080122]\n",
      " [ 0.21367409  7.79983443 10.38097119]\n",
      " [-2.74817152  7.56986467  9.40080122]\n",
      " [ 0.20475303  6.92460997  9.40080122]\n",
      " [ 0.04570783  7.3398949   8.42063124]]\n",
      "sigmax = \n",
      "[[ 0.09967165  7.66936796  9.41269486]\n",
      " [ 2.97753414  7.47070129  9.41525767]\n",
      " [ 0.02460958  8.31523439  9.40519946]\n",
      " [ 0.18365478  7.89798933 10.40116969]\n",
      " [-2.77819083  7.47690225  9.36337993]\n",
      " [ 0.17473372  7.02294115  9.42012328]\n",
      " [ 0.01568852  7.44004518  8.42413619]]\n",
      "sqP = \n",
      "[[1.5078776449033284, 0.0, 0.0], [-0.006332420264357737, 0.5697380738197144, 0.0], [0.026233452006272265, 0.09896065781701835, 0.6588583313722316]]\n",
      "m = \n",
      "[0.09722608 7.6795789  9.36375925]\n",
      "P = \n",
      "[[ 2.27338303 -0.00768779  0.03253562]\n",
      " [-0.00768779  0.10138332  0.04125377]\n",
      " [ 0.03253562  0.04125377  0.27132398]]\n",
      "num = \n",
      "729\n",
      "SENSOR 9 2.97631712843 -2.59124598555\n",
      "\n",
      "num = \n",
      "730\n",
      "SENSOR 6 1.83278194557 -0.45058413267\n",
      "\n",
      "num = \n",
      "731\n",
      "ODOMETRY -0.0102760318389 0.0999106615633 -0.0196442889398\n",
      "\n",
      "sqP = \n",
      "[[1.5077741969092773, 0.0, 0.0], [-0.005098766099271225, 0.3183666510376792, 0.0], [0.021578574667282895, 0.12992502592249056, 0.5039621330450064]]\n",
      "sigma = \n",
      "[[ 0.09722608  7.6795789   9.36375925]\n",
      " [ 3.11277448  7.6795789   9.36375925]\n",
      " [ 0.08702855  8.31631221  9.36375925]\n",
      " [ 0.14038323  7.93942896 10.37168351]\n",
      " [-2.91832231  7.6795789   9.36375925]\n",
      " [ 0.10742361  7.0428456   9.36375925]\n",
      " [ 0.05406893  7.41972885  8.35583498]]\n",
      "sigmax = \n",
      "[[ 0.0684025   7.77902482  9.37252384]\n",
      " [ 3.0839509   7.5798201   9.36756561]\n",
      " [ 0.05820497  8.41584232  9.3715093 ]\n",
      " [ 0.11155965  8.03840413 10.38473041]\n",
      " [-2.94714589  7.58202371  9.34256277]\n",
      " [ 0.07860004  7.14219697  9.37353747]\n",
      " [ 0.02524535  7.5194603   8.36030094]]\n",
      "sqP = \n",
      "[[1.572855575431458, 0.0, 0.0], [0.00021958579608537967, 0.5705904208437866, 0.0], [0.01293718421003386, 0.11591660518949924, 0.6653097811683284]]\n",
      "m = \n",
      "[0.06750484 7.84638677 9.31951177]\n",
      "P = \n",
      "[[ 2.47380783e+00 -1.85009339e-04  1.69102042e-02]\n",
      " [-1.85009339e-04  1.00605464e-01  4.32413546e-02]\n",
      " [ 1.69102042e-02  4.32413546e-02  2.79274954e-01]]\n",
      "num = \n",
      "732\n",
      "SENSOR 9 3.00538927132 -2.47582875838\n",
      "\n",
      "num = \n",
      "733\n",
      "SENSOR 5 4.86844686309 -0.644513040378\n",
      "\n",
      "num = \n",
      "734\n",
      "SENSOR 6 1.75818612283 -0.557237052247\n",
      "\n",
      "num = \n",
      "735\n",
      "ODOMETRY -0.0104218597473 0.100145397313 -0.0200408029886\n",
      "\n",
      "sqP = \n",
      "[[1.5728343308040045, 0.0, 0.0], [-0.00011762798874101089, 0.31718362175015113, 0.0], [0.010751421092413485, 0.13633307757165888, 0.5104631753141899]]\n",
      "sigma = \n",
      "[[ 0.06750484  7.84638677  9.31951177]\n",
      " [ 3.2131735   7.84638677  9.31951177]\n",
      " [ 0.06726958  8.48075401  9.31951177]\n",
      " [ 0.08900768  8.11905292 10.34043812]\n",
      " [-3.07816382  7.84638677  9.31951177]\n",
      " [ 0.06774009  7.21201953  9.31951177]\n",
      " [ 0.046002    7.57372061  8.29858542]]\n",
      "sigmax = \n",
      "[[ 0.03758452  7.94613386  9.32522642]\n",
      " [-3.09993213  7.74666379  9.3133906 ]\n",
      " [ 0.03734926  8.58050245  9.32520295]\n",
      " [ 0.05908736  8.21865409 10.34829613]\n",
      " [-3.10808414  7.74661721  9.31420374]\n",
      " [ 0.03781977  7.31176527  9.32524989]\n",
      " [ 0.01608168  7.67356752  8.30215407]]\n",
      "sqP = \n",
      "[[1.4320162658142002, 0.0, 0.0], [0.0830539379865469, 0.5654549986842933, 0.0], [0.008541413971939173, 0.122772176086301, 0.6682418372997663]]\n",
      "m = \n",
      "[-0.74314287  7.92665574  9.30790124]\n",
      "P = \n",
      "[[2.04601743 0.08728593 0.01255544]\n",
      " [0.08728593 0.10862866 0.0439112 ]\n",
      " [0.01255544 0.0439112  0.16773392]]\n",
      "num = \n",
      "736\n",
      "SENSOR 9 3.06790211886 -2.40968598481\n",
      "\n",
      "num = \n",
      "737\n",
      "SENSOR 5 4.81572738233 -0.616401204908\n",
      "\n",
      "num = \n",
      "738\n",
      "SENSOR 6 1.68748831042 -0.389912308279\n",
      "\n",
      "num = \n",
      "739\n",
      "ODOMETRY -0.0101350745755 0.100091285908 -0.0206119427088\n",
      "\n",
      "sqP = \n",
      "[[1.4303906579165673, 0.0, 0.0], [0.061022444300123185, 0.3238902921341929, 0.0], [0.008777631017312397, 0.1339205622616866, 0.3869394738927713]]\n",
      "sigma = \n",
      "[[-0.74314287  7.92665574  9.30790124]\n",
      " [ 2.11763845  7.92665574  9.30790124]\n",
      " [-0.62109798  8.57443633  9.30790124]\n",
      " [-0.7255876   8.19449687 10.08178018]\n",
      " [-3.60392418  7.92665574  9.30790124]\n",
      " [-0.86518775  7.27887516  9.30790124]\n",
      " [-0.76069813  7.65881462  8.53402229]]\n",
      "sigmax = \n",
      "[[-0.77360553  7.99968721  9.23937748]\n",
      " [ 2.08717579  7.8754752   9.39398058]\n",
      " [-0.65156064  8.6552668   9.24877818]\n",
      " [-0.75605027  8.26871997 10.01454901]\n",
      " [ 2.64879846  7.8374945   9.35350138]\n",
      " [-0.89565042  7.34302117  9.23099617]\n",
      " [-0.79116079  7.73063194  8.46422707]]\n",
      "sqP = \n",
      "[[1.4401615751616426, 0.0, 0.0], [-0.044701626442061715, 0.5724050628065854, 0.0], [0.05644985989727938, 0.09192360152139717, 0.5848765854133359]]\n",
      "m = \n",
      "[0.00991239 7.99774215 9.28021774]\n",
      "P = \n",
      "[[ 2.07034128 -0.04779988  0.05896527]\n",
      " [-0.04779988  0.10912952  0.04418   ]\n",
      " [ 0.05896527  0.04418     0.14419826]]\n",
      "num = \n",
      "740\n",
      "SENSOR 9 3.05939885103 -2.42259296594\n",
      "\n",
      "num = \n",
      "741\n",
      "SENSOR 5 4.85625275968 -0.554387962083\n",
      "\n",
      "num = \n",
      "742\n",
      "SENSOR 6 1.69976007626 -0.4523568891\n",
      "\n",
      "num = \n",
      "743\n",
      "ODOMETRY -0.0112125747693 0.100293480859 -0.0200290197221\n",
      "\n",
      "sqP = \n",
      "[[1.4388680564075318, 0.0, 0.0], [-0.03322047316098192, 0.3286729696565355, 0.0], [0.04098031856883206, 0.13856139091152, 0.35116892154498447]]\n",
      "sigma = \n",
      "[[ 9.91239186e-03  7.99774215e+00  9.28021774e+00]\n",
      " [ 2.88764850e+00  7.99774215e+00  9.28021774e+00]\n",
      " [-5.65285545e-02  8.65508809e+00  9.28021774e+00]\n",
      " [ 9.18730290e-02  8.27486494e+00  9.98255558e+00]\n",
      " [-2.86782372e+00  7.99774215e+00  9.28021774e+00]\n",
      " [ 7.63533382e-02  7.34039621e+00  9.28021774e+00]\n",
      " [-7.20482453e-02  7.72061937e+00  8.57787990e+00]]\n",
      "sigmax = \n",
      "[[-0.02083463  8.09783344  9.28019545]\n",
      " [ 2.85690149  7.90112071  9.30634362]\n",
      " [-0.08727557  8.75495706  9.27355023]\n",
      " [ 0.06112601  8.37462205  9.99072773]\n",
      " [-2.89857074  7.90110908  9.25413489]\n",
      " [ 0.04560632  7.44026814  9.28684077]\n",
      " [-0.10279526  7.82037283  8.56966332]]\n",
      "sqP = \n",
      "[[1.5076883862711916, 0.0, 0.0], [-0.0034729574374044608, 0.5783209518394872, 0.0], [0.022186059517145532, 0.08336185704919753, 0.5647601114736894]]\n",
      "m = \n",
      "[-0.01940405  8.01820334  9.31128777]\n",
      "P = \n",
      "[[ 2.27280291 -0.00402024  0.02563157]\n",
      " [-0.00402024  0.11035105  0.04489723]\n",
      " [ 0.02563157  0.04489723  0.13128875]]\n",
      "num = \n",
      "744\n",
      "SENSOR 9 3.36060085358 -2.4353356474\n",
      "\n",
      "num = \n",
      "745\n",
      "SENSOR 5 4.60467409396 -0.689671837769\n",
      "\n",
      "num = \n",
      "746\n",
      "SENSOR 6 1.66466459286 -0.434947234979\n",
      "\n",
      "num = \n",
      "747\n",
      "ODOMETRY -0.00936003709598 0.0997788840301 -0.0195655025372\n",
      "\n",
      "sqP = \n",
      "[[1.507581809425341, 0.0, 0.0], [-0.002666682915556822, 0.332180577248182, 0.0], [0.01700177915449132, 0.1352955811000294, 0.3357004544123437]]\n",
      "sigma = \n",
      "[[-0.01940405  8.01820334  9.31128777]\n",
      " [ 2.99575957  8.01820334  9.31128777]\n",
      " [-0.02473741  8.68256449  9.31128777]\n",
      " [ 0.01459951  8.2887945   9.98268868]\n",
      " [-3.03456767  8.01820334  9.31128777]\n",
      " [-0.01407068  7.35384219  9.31128777]\n",
      " [-0.05340761  7.74761218  8.63988686]]\n",
      "sigmax = \n",
      "[[-0.05064564  8.11844982  9.3082176 ]\n",
      " [ 2.96451798  7.91914411  9.32697376]\n",
      " [-0.05597901  8.78279317  9.307683  ]\n",
      " [-0.01664208  8.38908741  9.98302837]\n",
      " [-3.06580926  7.91836986  9.30169311]\n",
      " [-0.04531228  7.45410361  9.3087523 ]\n",
      " [-0.0846492   7.84769633  8.63341039]]\n",
      "sqP = \n",
      "[[1.5726090053800326, 0.0, 0.0], [0.0010855628035905153, 0.5797688104008355, 0.0], [0.009707024355932636, 0.07820339214803279, 0.5546880417902539]]\n",
      "m = \n",
      "[-0.05094127  8.2190483   9.27624182]\n",
      "P = \n",
      "[[2.47304214e+00 1.25240290e-03 1.20107191e-02]\n",
      " [1.25240290e-03 1.11358872e-01 4.33839385e-02]\n",
      " [1.20107191e-02 4.33839385e-02 1.25274569e-01]]\n",
      "num = \n",
      "748\n",
      "SENSOR 9 3.24434042861 -2.50085985671\n",
      "\n",
      "num = \n",
      "749\n",
      "SENSOR 5 4.44097432373 -0.539545027585\n",
      "\n",
      "num = \n",
      "750\n",
      "SENSOR 6 1.44534873942 -0.340798495725\n",
      "\n",
      "num = \n",
      "751\n",
      "ODOMETRY -0.0105609095644 0.100090264715 -0.0197529740061\n",
      "\n",
      "sqP = \n",
      "[[1.572590898949263, 0.0, 0.0], [0.0007963945986079854, 0.3337038169117282, 0.0], [0.007637535672634689, 0.12998909143668722, 0.32911863127433216]]\n",
      "sigma = \n",
      "[[-0.05094127  8.2190483   9.27624182]\n",
      " [ 3.09424053  8.2190483   9.27624182]\n",
      " [-0.04934848  8.88645593  9.27624182]\n",
      " [-0.0356662   8.47902648  9.93447908]\n",
      " [-3.19612307  8.2190483   9.27624182]\n",
      " [-0.05253406  7.55164066  9.27624182]\n",
      " [-0.06621634  7.95907012  8.61800455]]\n",
      "sigmax = \n",
      "[[-0.07986681  8.31864583  9.27022866]\n",
      " [ 3.06531499  8.11942983  9.28189746]\n",
      " [-0.07827402  8.98606291  9.27038731]\n",
      " [-0.06459174  8.57870424  9.92998793]\n",
      " [ 3.0581367   8.11947299  9.2826124 ]\n",
      " [-0.0814596   7.65122849  9.27007003]\n",
      " [-0.09514188  8.05856418  8.6104708 ]]\n",
      "sqP = \n",
      "[[1.4319961633639176, 0.0, 0.0], [-0.08105747871988823, 0.5737069238289194, 0.0], [0.0067061090339177705, 0.07498037152697595, 0.5506182595553271]]\n",
      "m = \n",
      "[0.70385503 8.246318   9.11858373]\n",
      "P = \n",
      "[[ 2.04617639 -0.08587425  0.00419925]\n",
      " [-0.08587425  0.12101207  0.03819786]\n",
      " [ 0.00419925  0.03819786  0.11762832]]\n",
      "num = \n",
      "752\n",
      "SENSOR 9 3.43181578801 -2.38831289505\n",
      "\n",
      "num = \n",
      "753\n",
      "SENSOR 5 4.47137535155 -0.631269209482\n",
      "\n",
      "num = \n",
      "754\n",
      "SENSOR 6 1.26851574319 -0.398939844547\n",
      "\n",
      "num = \n",
      "755\n",
      "ODOMETRY -0.0107658587575 0.0999709096433 -0.0195658499488\n",
      "\n",
      "sqP = \n",
      "[[1.4304462199127024, 0.0, 0.0], [-0.06003319061444178, 0.3426486402947775, 0.0], [0.00293562467193741, 0.11199254956858679, 0.3241563935155279]]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "sigma = \n",
      "[[ 0.70385503  8.246318    9.11858373]\n",
      " [ 3.56474747  8.246318    9.11858373]\n",
      " [ 0.58378865  8.93161528  9.11858373]\n",
      " [ 0.70972628  8.4703031   9.76689652]\n",
      " [-2.15703741  8.246318    9.11858373]\n",
      " [ 0.82392141  7.56102072  9.11858373]\n",
      " [ 0.69798378  8.0223329   8.47027094]]\n",
      "sigmax = \n",
      "[[ 0.67354114  8.32330192  9.18254885]\n",
      " [-2.74875172  8.15462693  9.07844884]\n",
      " [ 0.55347476  9.01570659  9.17286735]\n",
      " [ 0.67941239  8.54691014  9.83131252]\n",
      " [-2.1873513   8.19006725  9.03579536]\n",
      " [ 0.79360752  7.62978879  9.19130933]\n",
      " [ 0.66766989  8.09969105  8.53378297]]\n",
      "sqP = \n",
      "[[1.4400908233766585, 0.0, 0.0], [0.04831786687814112, 0.5784067830023014, 0.0], [0.05105529194896353, 0.06163502507357612, 0.5494505496706362]]\n",
      "m = \n",
      "[-0.11078651  8.37119086  9.07813718]\n",
      "P = \n",
      "[[2.07112162 0.05183258 0.05847654]\n",
      " [0.05183258 0.11916799 0.04335986]\n",
      " [0.05847654 0.04335986 0.12295494]]\n",
      "num = \n",
      "756\n",
      "SENSOR 9 3.50551920675 -2.39732140232\n",
      "\n",
      "num = \n",
      "757\n",
      "SENSOR 5 4.28281556549 -0.589616154158\n",
      "\n",
      "num = \n",
      "758\n",
      "SENSOR 6 1.28831954512 -0.513359363001\n",
      "\n",
      "num = \n",
      "759\n",
      "ODOMETRY -0.0101758354185 0.10002526256 -0.0193481524309\n",
      "\n",
      "sqP = \n",
      "[[1.4391391931502135, 0.0, 0.0], [0.03601637989797693, 0.34332319238638265, 0.0], [0.04063300119327056, 0.12203196845757118, 0.3262086697883991]]\n",
      "sigma = \n",
      "[[-0.11078651  8.37119086  9.07813718]\n",
      " [ 2.76749188  8.37119086  9.07813718]\n",
      " [-0.03875375  9.05783725  9.07813718]\n",
      " [-0.02952051  8.6152548   9.73055452]\n",
      " [-2.9890649   8.37119086  9.07813718]\n",
      " [-0.18281927  7.68454448  9.07813718]\n",
      " [-0.19205251  8.12712693  8.42571984]]\n",
      "sigmax = \n",
      "[[-0.14111822  8.47042415  9.06601538]\n",
      " [ 2.73716017  8.27853298  9.11566981]\n",
      " [-0.06908546  9.15768561  9.07318869]\n",
      " [-0.05985222  8.7151446   9.72652815]\n",
      " [-3.0193966   8.2722228   9.06401254]\n",
      " [-0.21315098  7.78264802  9.05890495]\n",
      " [-0.22238422  8.22504871  8.40558263]]\n",
      "sqP = \n",
      "[[1.5080019355650627, 0.0, 0.0], [0.013017085370835495, 0.5832105328333769, 0.0], [0.021308069717868798, 0.06955090538703206, 0.5514077080616712]]\n",
      "m = \n",
      "[-0.14306203  8.45800506  8.99500406]\n",
      "P = \n",
      "[[2.27371877 0.01470165 0.02540407]\n",
      " [0.01470165 0.11965984 0.04074451]\n",
      " [0.02540407 0.04074451 0.12159902]]\n",
      "num = \n",
      "760\n",
      "SENSOR 9 3.67081338849 -2.3789885528\n",
      "\n",
      "num = \n",
      "761\n",
      "SENSOR 5 4.40168128717 -0.489968693872\n",
      "\n",
      "num = \n",
      "762\n",
      "SENSOR 6 1.19631205076 -0.512575604526\n",
      "\n",
      "num = \n",
      "763\n",
      "ODOMETRY -0.0110076328184 0.100028655375 -0.0196243037287\n",
      "\n",
      "sqP = \n",
      "[[1.5078855308966157, 0.0, 0.0], [0.009749845186607996, 0.3457814030709774, 0.0], [0.016847477093563924, 0.11735809044572902, 0.327936371635159]]\n",
      "sigma = \n",
      "[[-0.14306203  8.45800506  8.99500406]\n",
      " [ 2.87270904  8.45800506  8.99500406]\n",
      " [-0.12356234  9.14956786  8.99500406]\n",
      " [-0.10936707  8.69272124  9.6508768 ]\n",
      " [-3.15883309  8.45800506  8.99500406]\n",
      " [-0.16256172  7.76644225  8.99500406]\n",
      " [-0.17675698  8.22328888  8.33913132]]\n",
      "sigmax = \n",
      "[[-0.17258601  8.55685823  8.97973632]\n",
      " [ 2.84318505  8.36184928  9.02255618]\n",
      " [-0.15308632  9.24869994  8.9816667 ]\n",
      " [-0.13889106  8.79203265  9.63894795]\n",
      " [ 3.09482823  8.35801738  8.99774603]\n",
      " [-0.1920857   7.86497893  8.97781174]\n",
      " [-0.20628097  8.32157159  8.32054202]]\n",
      "sqP = \n",
      "[[1.433487875920264, 0.0, 0.0], [-0.07721698466963173, 0.5786266857968604, 0.0], [0.016104538734567096, 0.06779207852504206, 0.5513572530105038]]\n",
      "m = \n",
      "[0.60464545 8.5834818  9.03027689]\n",
      "P = \n",
      "[[ 2.05067951 -0.08208779  0.01451203]\n",
      " [-0.08208779  0.12658405  0.03839516]\n",
      " [ 0.01451203  0.03839516  0.12210862]]\n",
      "num = \n",
      "764\n",
      "SENSOR 9 3.62146176736 -2.30271042095\n",
      "\n",
      "num = \n",
      "765\n",
      "SENSOR 5 4.18233806232 -0.556833847024\n",
      "\n",
      "num = \n",
      "766\n",
      "SENSOR 6 1.01671369638 -0.459323796962\n",
      "\n",
      "num = \n",
      "767\n",
      "ODOMETRY -0.00955580720005 0.0998135945322 -0.0197517141062\n",
      "\n",
      "sqP = \n",
      "[[1.4320193818626514, 0.0, 0.0], [-0.057323097512677855, 0.3511383163947699, 0.0], [0.010133964659910713, 0.11099920538433819, 0.33118740390399265]]\n",
      "sigma = \n",
      "[[ 0.60464545  8.5834818   9.03027689]\n",
      " [ 3.46868421  8.5834818   9.03027689]\n",
      " [ 0.48999925  9.28575844  9.03027689]\n",
      " [ 0.62491338  8.80548021  9.6926517 ]\n",
      " [-2.25939332  8.5834818   9.03027689]\n",
      " [ 0.71929164  7.88120517  9.03027689]\n",
      " [ 0.58437752  8.36148339  8.36790208]]\n",
      "sigmax = \n",
      "[[ 0.57401351  8.66639668  9.08623093]\n",
      " [-2.84513303  8.48840857  8.99918329]\n",
      " [ 0.45936732  9.37452988  9.07637855]\n",
      " [ 0.59428144  8.88724407  9.75027465]\n",
      " [-2.29002525  8.51907183  8.95374528]\n",
      " [ 0.68865971  7.95717486  9.09534868]\n",
      " [ 0.55374558  8.44551524  8.42216424]]\n",
      "sqP = \n",
      "[[1.4398605836667515, 0.0, 0.0], [0.05387571477114081, 0.5829723410743594, 0.0], [0.046284819398838425, 0.06131573642111253, 0.5538464736896317]]\n",
      "m = \n",
      "[-0.22408985  8.61162252  8.90305766]\n",
      "P = \n",
      "[[2.07022828 0.05723802 0.05176872]\n",
      " [0.05723802 0.12542294 0.034827  ]\n",
      " [0.05176872 0.034827   0.11412137]]\n",
      "num = \n",
      "768\n",
      "SENSOR 9 3.70417031979 -2.34473876598\n",
      "\n",
      "num = \n",
      "769\n",
      "SENSOR 5 4.17062919093 -0.570876489648\n",
      "\n",
      "num = \n",
      "770\n",
      "SENSOR 6 0.923913647873 -0.520206587019\n",
      "\n",
      "num = \n",
      "771\n",
      "ODOMETRY -0.0091535851559 0.100020863652 -0.0201880877397\n",
      "\n",
      "sqP = \n",
      "[[1.4388287862436215, 0.0, 0.0], [0.03978098166730155, 0.35190966729081274, 0.0], [0.03597976732743232, 0.09489847682440254, 0.32221282708929877]]\n",
      "sigma = \n",
      "[[-0.22408985  8.61162252  8.90305766]\n",
      " [ 2.65356772  8.61162252  8.90305766]\n",
      " [-0.14452789  9.31544185  8.90305766]\n",
      " [-0.15213032  8.80141947  9.54748332]\n",
      " [-3.10174743  8.61162252  8.90305766]\n",
      " [-0.30365182  7.90780318  8.90305766]\n",
      " [-0.29604939  8.42182556  8.25863201]]\n",
      "sigmax = \n",
      "[[-0.25339737  8.70872406  8.87994826]\n",
      " [ 2.6242602   8.52391234  8.9506988 ]\n",
      " [-0.17383541  9.41407291  8.8877388 ]\n",
      " [-0.18143784  8.89993122  9.53141507]\n",
      " [-3.13105495  8.51185471  8.90003483]\n",
      " [-0.33295934  8.00276086  8.87230392]\n",
      " [-0.32535691  8.5170143   8.22860106]]\n",
      "sqP = \n",
      "[[1.5076821104870362, 0.0, 0.0], [0.014470806325654189, 0.5833239604417244, 0.0], [0.019962054959574045, 0.05257472302235497, 0.5508915811429222]]\n",
      "m = \n",
      "[-0.25521706  8.66767922  8.8326235 ]\n",
      "P = \n",
      "[[2.27276503 0.01662725 0.02384976]\n",
      " [0.01662725 0.1249179  0.03560263]\n",
      " [0.02384976 0.03560263 0.1188017 ]]\n",
      "num = \n",
      "772\n",
      "SENSOR 9 3.69578950794 -2.41413549551\n",
      "\n",
      "num = \n",
      "773\n",
      "SENSOR 5 4.00110518645 -0.494969326567\n",
      "\n",
      "num = \n",
      "774\n",
      "SENSOR 6 0.964762619449 -0.53458348217\n",
      "\n",
      "num = \n",
      "775\n",
      "ODOMETRY -0.010295675802 0.100004015507 -0.0189695088639\n",
      "\n",
      "sqP = \n",
      "[[1.507569244737819, 0.0, 0.0], [0.011029181134548783, 0.35326513283745825, 0.0], [0.01582001040866123, 0.10028769609004784, 0.3293839746734749]]\n",
      "sigma = \n",
      "[[-0.25521706  8.66767922  8.8326235 ]\n",
      " [ 2.75992143  8.66767922  8.8326235 ]\n",
      " [-0.2331587   9.37420949  8.8326235 ]\n",
      " [-0.22357704  8.86825462  9.49139145]\n",
      " [-3.27035555  8.66767922  8.8326235 ]\n",
      " [-0.27727543  7.96114896  8.8326235 ]\n",
      " [-0.28685708  8.46710383  8.17385555]]\n",
      "sigmax = \n",
      "[[-0.28455874  8.76422508  8.80648786]\n",
      " [ 2.73057975  8.57520042  8.87072656]\n",
      " [-0.26250037  9.47130832  8.80862369]\n",
      " [-0.25291872  8.96557894  9.4683231 ]\n",
      " [ 2.98348808  8.5686081   8.84637433]\n",
      " [-0.3066171   8.05709486  8.80436475]\n",
      " [-0.31619876  8.56277457  8.14467879]]\n",
      "sqP = \n",
      "[[1.4334993563568614, 0.0, 0.0], [-0.07525913416474048, 0.5800110883937319, 0.0], [0.02478229726823105, 0.05809847220025477, 0.5532258475777929]]\n",
      "m = \n",
      "[0.50096687 8.68051056 8.74258689]\n",
      "P = \n",
      "[[ 2.05068587 -0.08030345  0.02429955]\n",
      " [-0.08030345  0.13055281  0.0341306 ]\n",
      " [ 0.02429955  0.0341306   0.12300264]]\n",
      "num = \n",
      "776\n",
      "SENSOR 9 3.69844459713 -2.31420630353\n",
      "\n",
      "num = \n",
      "777\n",
      "SENSOR 5 3.93813832417 -0.404459278547\n",
      "\n",
      "num = \n",
      "778\n",
      "SENSOR 6 0.762339598312 -0.528477333035\n",
      "\n",
      "num = \n",
      "779\n",
      "ODOMETRY -0.00986627304745 0.0999308954255 -0.0194932169016\n",
      "\n",
      "sqP = \n",
      "[[1.4320216016785683, 0.0, 0.0], [-0.056076980978776454, 0.3569428241998755, 0.0], [0.016968706277632, 0.09828508179592872, 0.3362361448436491]]\n",
      "sigma = \n",
      "[[ 0.50096687  8.68051056  8.74258689]\n",
      " [ 3.36501007  8.68051056  8.74258689]\n",
      " [ 0.38881291  9.39439621  8.74258689]\n",
      " [ 0.53490428  8.87708073  9.41505918]\n",
      " [-2.36307633  8.68051056  8.74258689]\n",
      " [ 0.61312083  7.96662491  8.74258689]\n",
      " [ 0.46702946  8.4839404   8.0701146 ]]\n",
      "sigmax = \n",
      "[[ 0.47170168  8.76871578  8.78971058]\n",
      " [-2.94744042  8.5827691   8.72143484]\n",
      " [ 0.35954772  9.4873213   8.77954268]\n",
      " [ 0.5056391   8.96363621  9.46514862]\n",
      " [-2.39234152  8.60859285  8.67309843]\n",
      " [ 0.58385565  8.04900193  8.79928636]\n",
      " [ 0.43776427  8.57369378  8.11421828]]\n",
      "sqP = \n",
      "[[1.439875624602277, 0.0, 0.0], [0.05847248460695856, 0.5839731755123758, 0.0], [0.04078215448939502, 0.054247103784511634, 0.5578766800770399]]\n",
      "m = \n",
      "[-0.32523409  8.68745647  8.64424834]\n",
      "P = \n",
      "[[2.07040766 0.06306749 0.0473764 ]\n",
      " [0.06306749 0.12991781 0.03965935]\n",
      " [0.0473764  0.03965935 0.1279537 ]]\n",
      "num = \n",
      "780\n",
      "SENSOR 9 3.97863915035 -2.33817010298\n",
      "\n",
      "num = \n",
      "781\n",
      "SENSOR 5 3.91815869731 -0.375382277621\n",
      "\n",
      "num = \n",
      "782\n",
      "SENSOR 6 0.764457865234 -0.692422479677\n",
      "\n",
      "num = \n",
      "783\n",
      "ODOMETRY -0.0105039330611 0.100062635978 -0.0196007510172\n",
      "\n",
      "sqP = \n",
      "[[1.4388911204275838, 0.0, 0.0], [0.04383062089842027, 0.3577662482468294, 0.0], [0.032925634855795805, 0.10681890527056534, 0.33979305699897744]]\n",
      "sigma = \n",
      "[[-0.32523409  8.68745647  8.64424834]\n",
      " [ 2.55254815  8.68745647  8.64424834]\n",
      " [-0.23757285  9.40298897  8.64424834]\n",
      " [-0.25938282  8.90109428  9.32383446]\n",
      " [-3.20301633  8.68745647  8.64424834]\n",
      " [-0.41289533  7.97192397  8.64424834]\n",
      " [-0.39108536  8.47381866  7.96466223]]\n",
      "sigmax = \n",
      "[[-0.35459358  8.78182894  8.61138467]\n",
      " [ 2.52318866  8.60491853  8.7005837 ]\n",
      " [-0.26693234  9.49987625  8.61977308]\n",
      " [-0.28874231  8.99742476  9.29725207]\n",
      " [ 3.05080949  8.5877794   8.65136638]\n",
      " [-0.44225482  8.06305689  8.60324864]\n",
      " [-0.42044485  8.56582403  7.92565973]]\n",
      "sqP = \n",
      "[[1.4390813143912529, 0.0, 0.0], [-0.0628274022535681, 0.5858148601366606, 0.0], [0.03325476029402868, 0.06552068160534372, 0.5595796312597069]]\n",
      "m = \n",
      "[0.41795133 8.8561258  8.62901535]\n",
      "P = \n",
      "[[ 2.06747325 -0.06696933  0.03441727]\n",
      " [-0.06696933  0.13186187  0.04126352]\n",
      " [ 0.03441727  0.04126352  0.13924217]]\n",
      "num = \n",
      "784\n",
      "SENSOR 9 3.99049095935 -2.43100832721\n",
      "\n",
      "num = \n",
      "785\n",
      "SENSOR 5 3.84981410268 -0.339858112469\n",
      "\n",
      "num = \n",
      "786\n",
      "SENSOR 6 0.670707796904 -0.627865459926\n",
      "\n",
      "num = \n",
      "787\n",
      "ODOMETRY -0.0102670061231 0.100220134545 -0.0197649412615\n",
      "\n",
      "sqP = \n",
      "[[1.4378710818378124, 0.0, 0.0], [-0.04657533362212105, 0.36012859674870507, 0.0], [0.023936272427809914, 0.11767564099110725, 0.3533011062161214]]\n",
      "sigma = \n",
      "[[ 0.41795133  8.8561258   8.62901535]\n",
      " [ 3.29369349  8.8561258   8.62901535]\n",
      " [ 0.32480066  9.57638299  8.62901535]\n",
      " [ 0.46582387  9.09147708  9.33561756]\n",
      " [-2.45779083  8.8561258   8.62901535]\n",
      " [ 0.511102    8.1358686   8.62901535]\n",
      " [ 0.37007878  8.62077451  7.92241314]]\n",
      "sigmax = \n",
      "[[ 0.38784664  8.94799684  8.66866687]\n",
      " [-3.0195965   8.7570646   8.61489409]\n",
      " [ 0.29469598  9.67154396  8.65994949]\n",
      " [ 0.43571919  9.18134537  9.37962007]\n",
      " [-2.48789552  8.77789988  8.56661954]\n",
      " [ 0.48099731  8.22365312  8.67704044]\n",
      " [ 0.3399741   8.7144378   7.95762281]]\n",
      "sqP = \n",
      "[[1.4390837893572792, 0.0, 0.0], [0.06452744054022398, 0.5887369539464307, 0.0], [0.03679719338670529, 0.06818752072239337, 0.5671649760831695]]\n",
      "m = \n",
      "[-0.3989453   8.91769849  8.59047149]\n",
      "P = \n",
      "[[2.06777698 0.06870931 0.04254579]\n",
      " [0.06870931 0.13223264 0.03868642]\n",
      " [0.04254579 0.03868642 0.13448831]]\n",
      "num = \n",
      "788\n",
      "SENSOR 9 3.97444895632 -2.34925609157\n",
      "\n",
      "num = \n",
      "789\n",
      "SENSOR 5 3.77657198716 -0.365748954501\n",
      "\n",
      "num = \n",
      "790\n",
      "SENSOR 6 0.724620745167 -0.68784406447\n",
      "\n",
      "num = \n",
      "791\n",
      "ODOMETRY -0.00978517807132 0.0997958111892 -0.0209122965862\n",
      "\n",
      "sqP = \n",
      "[[1.437976695896283, 0.0, 0.0], [0.047781934575730996, 0.36048512856511095, 0.0], [0.02958725774093793, 0.10339590397071054, 0.35060261179313806]]\n",
      "sigma = \n",
      "[[-0.3989453   8.91769849  8.59047149]\n",
      " [ 2.47700809  8.91769849  8.59047149]\n",
      " [-0.30338143  9.63866874  8.59047149]\n",
      " [-0.33977079  9.12449029  9.29167672]\n",
      " [-3.27489869  8.91769849  8.59047149]\n",
      " [-0.49450917  8.19672823  8.59047149]\n",
      " [-0.45811982  8.71090668  7.88926627]]\n",
      "sigmax = \n",
      "[[-0.42897725  9.0096439   8.55059523]\n",
      " [ 2.44697614  8.83944664  8.65308717]\n",
      " [-0.33341338  9.73399956  8.55955046]\n",
      " [-0.36980273  9.21863306  9.25730789]\n",
      " [ 2.97825467  8.81850951  8.60481102]\n",
      " [-0.52454112  8.28444918  8.54200388]\n",
      " [-0.48815176  8.80033288  7.84402215]]\n",
      "sqP = \n",
      "[[1.4392186764521664, 0.0, 0.0], [-0.06016634638658912, 0.5871754973486322, 0.0], [0.038329978076147415, 0.06515780399537559, 0.5660696774019933]]\n",
      "m = \n",
      "[0.35798487 8.92636795 8.5409894 ]\n",
      "P = \n",
      "[[ 2.06792029 -0.06463725  0.04031865]\n",
      " [-0.06463725  0.13261324  0.03399937]\n",
      " [ 0.04031865  0.03399937  0.13689918]]\n",
      "num = \n",
      "792\n",
      "SENSOR 9 4.03143217141 -2.26626563102\n",
      "\n",
      "num = \n",
      "793\n",
      "SENSOR 5 3.46349133261 -0.34264365269\n",
      "\n",
      "num = \n",
      "794\n",
      "SENSOR 6 0.475188697577 -0.867727704814\n",
      "\n",
      "num = \n",
      "795\n",
      "ODOMETRY -0.0108972815108 0.0998936621075 -0.0198820412631\n",
      "\n",
      "sqP = \n",
      "[[1.4380265264910177, 0.0, 0.0], [-0.044948577155485975, 0.3613763463945508, 0.0], [0.028037486879859155, 0.09757035612769147, 0.355799254358837]]\n",
      "sigma = \n",
      "[[ 0.35798487  8.92636795  8.5409894 ]\n",
      " [ 3.23403792  8.92636795  8.5409894 ]\n",
      " [ 0.26808772  9.64912064  8.5409894 ]\n",
      " [ 0.41405984  9.12150866  9.25258791]\n",
      " [-2.51806818  8.92636795  8.5409894 ]\n",
      " [ 0.44788202  8.20361525  8.5409894 ]\n",
      " [ 0.3019099   8.73122723  7.82939089]]\n",
      "sigmax = \n",
      "[[ 0.3272874   9.02017486  8.57504034]\n",
      " [-3.07984486  8.82691288  8.53274966]\n",
      " [ 0.23739024  9.74560572  8.56648122]\n",
      " [ 0.38336237  9.21325972  9.29184279]\n",
      " [-2.54876566  8.84478485  8.48351416]\n",
      " [ 0.41718455  8.29398641  8.58332446]\n",
      " [ 0.27121242  8.82679511  7.85813084]]\n",
      "sqP = \n",
      "[[1.4390917692544236, 0.0, 0.0], [0.06633606189292038, 0.5856726877493238, 0.0], [0.03333995411878101, 0.05658998854239149, 0.5702312471895596]]\n",
      "m = \n",
      "[-0.47027872  8.96241134  8.3283188 ]\n",
      "P = \n",
      "[[2.06783867 0.07138851 0.0390948 ]\n",
      " [0.07138851 0.13399726 0.03741512]\n",
      " [0.0390948  0.03741512 0.13641414]]\n",
      "num = \n",
      "796\n",
      "SENSOR 9 4.23985887378 -2.26854947811\n",
      "\n",
      "num = \n",
      "797\n",
      "SENSOR 5 3.41389965628 -0.282744929273\n",
      "\n",
      "num = \n",
      "798\n",
      "SENSOR 6 0.627709996436 -0.94148755275\n",
      "\n",
      "num = \n",
      "799\n",
      "ODOMETRY -0.0092026691367 0.0998946022284 -0.0202789115158\n",
      "\n",
      "sqP = \n",
      "[[1.437998145411189, 0.0, 0.0], [0.04964436652802743, 0.3626743699307895, 0.0], [0.02718696164369801, 0.09944302372666732, 0.3546633495811844]]\n",
      "sigma = \n",
      "[[-0.47027872  8.96241134  8.3283188 ]\n",
      " [ 2.40571757  8.96241134  8.3283188 ]\n",
      " [-0.37098998  9.68776008  8.3283188 ]\n",
      " [-0.41590479  9.16129738  9.0376455 ]\n",
      " [-3.34627501  8.96241134  8.3283188 ]\n",
      " [-0.56956745  8.2370626   8.3283188 ]\n",
      " [-0.52465264  8.76352529  7.6189921 ]]\n",
      "sigmax = \n",
      "[[-0.50105804  9.0509622   8.28208582]\n",
      " [ 2.37493825  8.88910086  8.39617392]\n",
      " [-0.40176931  9.78045769  8.29109118]\n",
      " [-0.44668412  9.25223001  8.99629333]\n",
      " [ 2.90613098  8.86482996  8.34968742]\n",
      " [-0.60034677  8.32059446  8.27353585]\n",
      " [-0.55543196  8.84943265  7.56801496]]\n",
      "sqP = \n",
      "[[1.4392323913094993, 0.0, 0.0], [-0.05720467334459756, 0.5880213267697502, 0.0], [0.04296463780716932, 0.06314295413995233, 0.5686510967381323]]\n",
      "m = \n",
      "[0.27222837 9.08844825 8.25601087]\n",
      "P = \n",
      "[[ 2.06792514 -0.0611345   0.04559336]\n",
      " [-0.0611345   0.13738809  0.04168863]\n",
      " [ 0.04559336  0.04168863  0.14901505]]\n",
      "num = \n",
      "800\n",
      "SENSOR 9 4.11518464247 -2.31925173137\n",
      "\n",
      "num = \n",
      "801\n",
      "SENSOR 5 3.37747667925 -0.382887053434\n",
      "\n",
      "num = \n",
      "802\n",
      "SENSOR 6 0.347854511055 -1.06521018247\n",
      "\n",
      "num = \n",
      "803\n",
      "ODOMETRY -0.00964561978411 0.0999128007624 -0.0203232411783\n",
      "\n",
      "sqP = \n",
      "[[1.4380282133960205, 0.0, 0.0], [-0.04251272833261663, 0.3682129306982574, 0.0], [0.031705469042088215, 0.11687941939953782, 0.36653651453099045]]\n",
      "sigma = \n",
      "[[ 0.27222837  9.08844825  8.25601087]\n",
      " [ 3.14828479  9.08844825  8.25601087]\n",
      " [ 0.18720291  9.82487412  8.25601087]\n",
      " [ 0.33563931  9.32220709  8.9890839 ]\n",
      " [-2.60382806  9.08844825  8.25601087]\n",
      " [ 0.35725382  8.35202239  8.25601087]\n",
      " [ 0.20881743  8.85468942  7.52293784]]\n",
      "sigmax = \n",
      "[[ 0.24274679  9.18490725  8.2819838 ]\n",
      " [ 3.11880321  8.98855397  8.25626165]\n",
      " [ 0.15772133  9.92319036  8.27369838]\n",
      " [ 0.30615772  9.41682636  9.02111709]\n",
      " [-2.63330964  9.00218595  8.20563481]\n",
      " [ 0.32777224  8.44592723  8.29008157]\n",
      " [ 0.17933585  8.95260041  7.54274611]]\n",
      "sqP = \n",
      "[[1.5068967155583066, 0.0, 0.0], [-0.011229562417214498, 0.5970525609959031, 0.0], [0.01997014173426988, 0.07264414971149884, 0.5758361073566298]]\n",
      "m = \n",
      "[0.24209273 9.04147027 8.20284837]\n",
      "P = \n",
      "[[ 2.27040044 -0.01277058  0.02309777]\n",
      " [-0.01277058  0.13470988  0.0425507 ]\n",
      " [ 0.02309777  0.0425507   0.14817171]]\n",
      "num = \n",
      "804\n",
      "SENSOR 9 4.2592528018 -2.25631821864\n",
      "\n",
      "num = \n",
      "805\n",
      "SENSOR 5 3.48949092568 -0.316251464921\n",
      "\n",
      "num = \n",
      "806\n",
      "SENSOR 6 0.418415784686 -1.24819855562\n",
      "\n",
      "num = \n",
      "807\n",
      "ODOMETRY -0.0090889765955 0.100078317535 -0.0208387649645\n",
      "\n",
      "sqP = \n",
      "[[1.506784802816363, 0.0, 0.0], [-0.008475386846491451, 0.366930572883061, 0.0], [0.015329176354600278, 0.11631797699037635, 0.36661540898005346]]\n",
      "sigma = \n",
      "[[ 0.24209273  9.04147027  8.20284837]\n",
      " [ 3.25566234  9.04147027  8.20284837]\n",
      " [ 0.22514196  9.77533141  8.20284837]\n",
      " [ 0.27275108  9.27410622  8.93607919]\n",
      " [-2.77147688  9.04147027  8.20284837]\n",
      " [ 0.2590435   8.30760912  8.20284837]\n",
      " [ 0.21143438  8.80883431  7.46961756]]\n",
      "sigmax = \n",
      "[[ 0.21212387  9.13869597  8.22586424]\n",
      " [-3.05749183  8.94210171  8.19243402]\n",
      " [ 0.1951731   9.87293327  8.22421296]\n",
      " [ 0.24278222  9.37058072  8.96206455]\n",
      " [-2.80144574  8.94797875  8.16760771]\n",
      " [ 0.22907464  8.40443074  8.2275089 ]\n",
      " [ 0.18146552  8.90671985  7.48964229]]\n",
      "sqP = \n",
      "[[1.433511463587305, 0.0, 0.0], [0.07870261545984497, 0.590819347905594, 0.0], [0.022499748648308636, 0.07154349206017471, 0.574726169419645]]\n",
      "m = \n",
      "[-0.56779011  9.12248324  8.26725286]\n",
      "P = \n",
      "[[2.05109022 0.08443925 0.02852366]\n",
      " [0.08443925 0.14162567 0.04742722]\n",
      " [0.02852366 0.04742722 0.15065109]]\n",
      "num = \n",
      "808\n",
      "SENSOR 9 4.38985383683 -2.35866286668\n",
      "\n",
      "num = \n",
      "809\n",
      "SENSOR 5 3.28107289655 -0.276801161352\n",
      "\n",
      "num = \n",
      "810\n",
      "SENSOR 6 0.520377085458 -1.46790593847\n",
      "\n",
      "num = \n",
      "811\n",
      "ODOMETRY -0.00947836960967 0.0999378658775 -0.0205443341273\n",
      "\n",
      "sqP = \n",
      "[[1.432162776406645, 0.0, 0.0], [0.0589592546078945, 0.3716846450521197, 0.0], [0.01991649432246813, 0.12444139262688408, 0.36710865446605023]]\n",
      "sigma = \n",
      "[[-0.56779011  9.12248324  8.26725286]\n",
      " [ 2.29653544  9.12248324  8.26725286]\n",
      " [-0.4498716   9.86585253  8.26725286]\n",
      " [-0.52795712  9.37136603  9.00147017]\n",
      " [-3.43211566  9.12248324  8.26725286]\n",
      " [-0.68570862  8.37911395  8.26725286]\n",
      " [-0.6076231   8.87360046  7.53303555]]\n",
      "sigmax = \n",
      "[[-0.59771785  9.20636578  8.21266907]\n",
      " [ 2.2666077   9.05674554  8.34271295]\n",
      " [-0.47979934  9.95557409  8.22291651]\n",
      " [-0.55788486  9.45735568  8.95027008]\n",
      " [ 2.8211419   9.0268633   8.29679092]\n",
      " [-0.71563636  8.45599244  8.20317972]\n",
      " [-0.63755084  8.95524279  7.47515465]]\n",
      "sqP = \n",
      "[[1.4400149120573447, 0.0, 0.0], [-0.0508600507363899, 0.5990302661418014, 0.0], [0.04794269986083693, 0.07907893755094202, 0.574235491678059]]\n",
      "m = \n",
      "[0.17540259 9.22617257 8.16506387]\n",
      "P = \n",
      "[[ 2.07038532 -0.05452527  0.05150612]\n",
      " [-0.05452527  0.13714427  0.04148328]\n",
      " [ 0.05150612  0.04148328  0.15102619]]\n",
      "num = \n",
      "812\n",
      "SENSOR 9 4.49482605041 -2.26889815025\n",
      "\n",
      "num = \n",
      "813\n",
      "SENSOR 5 3.15258240035 -0.336438423951\n",
      "\n",
      "num = \n",
      "814\n",
      "SENSOR 6 0.430980850097 -1.70329062466\n",
      "\n",
      "num = \n",
      "815\n",
      "ODOMETRY -0.00935420041505 0.100110622918 -0.0200634654475\n",
      "\n",
      "sqP = \n",
      "[[1.438883359016196, 0.0, 0.0], [-0.03789415911708681, 0.3683860735975561, 0.0], [0.03579589568481443, 0.11629033207182066, 0.3690818354484457]]\n",
      "sigma = \n",
      "[[ 0.17540259  9.22617257  8.16506387]\n",
      " [ 3.05316931  9.22617257  8.16506387]\n",
      " [ 0.09961428  9.96294472  8.16506387]\n",
      " [ 0.24699438  9.45875323  8.90322754]\n",
      " [-2.70236412  9.22617257  8.16506387]\n",
      " [ 0.25119091  8.48940042  8.16506387]\n",
      " [ 0.1038108   8.99359191  7.42690019]]\n",
      "sigmax = \n",
      "[[ 0.14537989  9.3247379   8.18157   ]\n",
      " [ 3.02314661  9.12671326  8.17483233]\n",
      " [ 0.06959157 10.06247689  8.17405966]\n",
      " [ 0.21697168  9.55588539  8.92674183]\n",
      " [-2.73238683  9.13532207  8.12342538]\n",
      " [ 0.22116821  8.58643304  8.18898557]\n",
      " [ 0.0737881   9.09308545  7.4363136 ]]\n",
      "sqP = \n",
      "[[1.5076812110976245, 0.0, 0.0], [-0.008581749404683023, 0.5970846054771097, 0.0], [0.02120564648585841, 0.072124073376029, 0.5775469810874402]]\n",
      "m = \n",
      "[0.14091749 9.31071591 8.07045689]\n",
      "P = \n",
      "[[ 2.27277336 -0.00982646  0.02458269]\n",
      " [-0.00982646  0.13430972  0.04073714]\n",
      " [ 0.02458269  0.04073714  0.1465561 ]]\n",
      "num = \n",
      "816\n",
      "SENSOR 9 4.56500399918 -2.3031466037\n",
      "\n",
      "num = \n",
      "817\n",
      "SENSOR 5 2.91571056826 -0.232623268414\n",
      "\n",
      "num = \n",
      "818\n",
      "SENSOR 6 0.330244890906 -1.90953716454\n",
      "\n",
      "num = \n",
      "819\n",
      "ODOMETRY -0.010269880911 0.100061171307 -0.0197644075107\n",
      "\n",
      "sqP = \n",
      "[[1.5075720068364944, 0.0, 0.0], [-0.006518072221598701, 0.36642493415485644, 0.0], [0.01630614769746349, 0.11146464021537958, 0.36587680898933156]]\n",
      "sigma = \n",
      "[[ 0.14091749  9.31071591  8.07045689]\n",
      " [ 3.1560615   9.31071591  8.07045689]\n",
      " [ 0.12788134 10.04356577  8.07045689]\n",
      " [ 0.17352978  9.53364519  8.80221051]\n",
      " [-2.87422653  9.31071591  8.07045689]\n",
      " [ 0.15395363  8.57786604  8.07045689]\n",
      " [ 0.10830519  9.08778663  7.33870327]]\n",
      "sigmax = \n",
      "[[ 0.11149982  9.40996138  8.08358981]\n",
      " [ 3.12664383  9.21060659  8.06994486]\n",
      " [ 0.09846368 10.14297401  8.08229495]\n",
      " [ 0.14411211  9.63240966  8.81857249]\n",
      " [-2.90364419  9.21391903  8.04491278]\n",
      " [ 0.12453596  8.67693188  8.08488243]\n",
      " [ 0.07888752  9.18740754  7.34859316]]\n",
      "sqP = \n",
      "[[1.5726034883323998, 0.0, 0.0], [-0.0011594203342293133, 0.5950066104003332, 0.0], [0.009812426486640389, 0.06995326094364586, 0.5746569319288863]]\n",
      "m = \n",
      "[0.10844004 9.3490286  7.89901585]\n",
      "P = \n",
      "[[ 2.47302376e+00 -1.40897852e-03  1.21288758e-02]\n",
      " [-1.40897852e-03  1.33884214e-01  4.07650188e-02]\n",
      " [ 1.21288758e-02  4.07650188e-02  1.44385195e-01]]\n",
      "num = \n",
      "820\n",
      "SENSOR 9 4.69167462854 -2.22658998309\n",
      "\n",
      "num = \n",
      "821\n",
      "SENSOR 5 3.08433569089 -0.337143319208\n",
      "\n",
      "num = \n",
      "822\n",
      "SENSOR 6 0.554762750925 -2.10973524603\n",
      "\n",
      "num = \n",
      "823\n",
      "ODOMETRY -0.00999199793928 0.0999665305154 -0.0202757591957\n",
      "\n",
      "sqP = \n",
      "[[1.572585055006257, 0.0, 0.0], [-0.0008959633161289446, 0.36590082158748805, 0.0], [0.007712699365238978, 0.1114289082270513, 0.3631932101161991]]\n",
      "sigma = \n",
      "[[ 0.10844004  9.3490286   7.89901585]\n",
      " [ 3.25361015  9.3490286   7.89901585]\n",
      " [ 0.10664812 10.08083024  7.89901585]\n",
      " [ 0.12386544  9.57188642  8.62540227]\n",
      " [-3.03673007  9.3490286   7.89901585]\n",
      " [ 0.11023197  8.61722696  7.89901585]\n",
      " [ 0.09301465  9.12617078  7.17262943]]\n",
      "sigmax = \n",
      "[[ 0.07840576  9.44860799  7.9088231 ]\n",
      " [-3.05960944  9.24948493  7.88885242]\n",
      " [ 0.07661383 10.18042705  7.90864464]\n",
      " [ 0.09383115  9.67130269  8.63674434]\n",
      " [-3.06676435  9.24941476  7.8895649 ]\n",
      " [ 0.08019768  8.71678862  7.90900152]\n",
      " [ 0.06298036  9.22588961  7.18089952]]\n",
      "sqP = \n",
      "[[1.4319966177455277, 0.0, 0.0], [0.0822924441374693, 0.5890280484695581, 0.0], [0.010028313518593917, 0.06859229026220118, 0.5724809715420003]]\n",
      "m = \n",
      "[-0.69935121  9.45323338  7.95344595]\n",
      "P = \n",
      "[[2.04649664 0.08836666 0.01483195]\n",
      " [0.08836666 0.14272102 0.0456516 ]\n",
      " [0.01483195 0.0456516  0.14412681]]\n",
      "num = \n",
      "824\n",
      "SENSOR 9 4.76284132902 -2.15126005988\n",
      "\n",
      "num = \n",
      "825\n",
      "SENSOR 5 2.95938070311 -0.158178168659\n",
      "\n",
      "num = \n",
      "826\n",
      "SENSOR 6 0.549503125028 -2.27808301514\n",
      "\n",
      "num = \n",
      "827\n",
      "ODOMETRY -0.00986005097223 0.100173854445 -0.0189859725838\n",
      "\n",
      "sqP = \n",
      "[[1.4305581581147402, 0.0, 0.0], [0.061770758488334764, 0.37270013432538923, 0.0], [0.010367943125508112, 0.1207704491411875, 0.35976911103024617]]\n",
      "sigma = \n",
      "[[-0.69935121  9.45323338  7.95344595]\n",
      " [ 2.16176511  9.45323338  7.95344595]\n",
      " [-0.57580969 10.19863365  7.95344595]\n",
      " [-0.67861532  9.69477428  8.67298417]\n",
      " [-3.56046753  9.45323338  7.95344595]\n",
      " [-0.82289273  8.70783311  7.95344595]\n",
      " [-0.7200871   9.21169248  7.23390773]]\n",
      "sigmax = \n",
      "[[-0.72961897  9.52908697  7.88833419]\n",
      " [ 2.13149735  9.39836768  8.03701066]\n",
      " [-0.60607745 10.28193267  7.89817769]\n",
      " [-0.70888308  9.77196161  8.60945919]\n",
      " [ 2.69245002  9.36232007  7.99501609]\n",
      " [-0.85316049  8.77508502  7.8794832 ]\n",
      " [-0.75035485  9.28617971  7.16723719]]\n",
      "sqP = \n",
      "[[1.4401877273494232, 0.0, 0.0], [-0.04460188814462239, 0.5990743777966907, 0.0], [0.05392606743783661, 0.07427593335219063, 0.5698735999795882]]\n",
      "m = \n",
      "[0.05121084 9.50990299 7.88954699]\n",
      "P = \n",
      "[[ 2.07087022 -0.04759424  0.05819178]\n",
      " [-0.04759424  0.13483539  0.04028557]\n",
      " [ 0.05819178  0.04028557  0.14525808]]\n",
      "num = \n",
      "828\n",
      "SENSOR 9 4.62199069994 -2.18236767895\n",
      "\n",
      "num = \n",
      "829\n",
      "SENSOR 5 2.88364841405 -0.211583718625\n",
      "\n",
      "num = \n",
      "830\n",
      "SENSOR 6 0.608371630443 -2.36582694156\n",
      "\n",
      "num = \n",
      "831\n",
      "ODOMETRY -0.0101721448458 0.0998079193853 -0.0198301989047\n",
      "\n",
      "sqP = \n",
      "[[1.4390518465089743, 0.0, 0.0], [-0.033073330187998407, 0.3657069108593289, 0.0], [0.04043758151417648, 0.11381513201730534, 0.36148166327772563]]\n",
      "sigma = \n",
      "[[ 0.05121084  9.50990299  7.88954699]\n",
      " [ 2.92931454  9.50990299  7.88954699]\n",
      " [-0.01493582 10.24131681  7.88954699]\n",
      " [ 0.13208601  9.73753325  8.61251032]\n",
      " [-2.82689285  9.50990299  7.88954699]\n",
      " [ 0.1173575   8.77848917  7.88954699]\n",
      " [-0.02966432  9.28227273  7.16658367]]\n",
      "sigmax = \n",
      "[[ 0.02236482  9.60999121  7.89368808]\n",
      " [ 2.90046851  9.41219055  7.91161687]\n",
      " [-0.04378184 10.34145987  7.88706335]\n",
      " [ 0.10323998  9.83695978  8.6247237 ]\n",
      " [-2.85573887  9.41434765  7.85948078]\n",
      " [ 0.08851148  8.87808479  7.9002947 ]\n",
      " [-0.05851034  9.38236835  7.16262539]]\n",
      "sqP = \n",
      "[[1.507845900650769, 0.0, 0.0], [-0.005491324641162793, 0.5949541183462055, 0.0], [0.02231459175266379, 0.06848613267679331, 0.5732478974742465]]\n",
      "m = \n",
      "[0.02101812 9.44139152 7.83050079]\n",
      "P = \n",
      "[[ 2.27327166 -0.00624838  0.02593148]\n",
      " [-0.00624838  0.12981179  0.03968599]\n",
      " [ 0.02593148  0.03968599  0.14083937]]\n",
      "num = \n",
      "832\n",
      "SENSOR 9 4.6158486866 -2.14373953498\n",
      "\n",
      "num = \n",
      "833\n",
      "SENSOR 5 2.58243352082 -0.200858606282\n",
      "\n",
      "num = \n",
      "834\n",
      "SENSOR 6 0.747480535627 -2.41041090005\n",
      "\n",
      "num = \n",
      "835\n",
      "ODOMETRY -0.0103893957533 0.0999764229641 -0.0201432882674\n",
      "\n",
      "sqP = \n",
      "[[1.507737265797724, 0.0, 0.0], [-0.0041442088467603215, 0.3602702027414335, 0.0], [0.017198940987773026, 0.11035402173645761, 0.3582813885959492]]\n",
      "sigma = \n",
      "[[ 0.02101812  9.44139152  7.83050079]\n",
      " [ 3.03649265  9.44139152  7.83050079]\n",
      " [ 0.0127297  10.16193193  7.83050079]\n",
      " [ 0.055416    9.66209957  8.54706357]\n",
      " [-2.99445642  9.44139152  7.83050079]\n",
      " [ 0.02930653  8.72085112  7.83050079]\n",
      " [-0.01337977  9.22068348  7.11393801]]\n",
      "sigmax = \n",
      "[[-8.98422868e-03  9.54119357e+00  7.83158328e+00]\n",
      " [ 3.00649030e+00  9.34224598e+00  7.84198040e+00]\n",
      " [-1.72726464e-02  1.02617395e+01  7.83075606e+00]\n",
      " [ 2.54136533e-02  9.76180535e+00  8.55157772e+00]\n",
      " [-3.02445876e+00  9.34251830e+00  7.81687339e+00]\n",
      " [-6.95810987e-04  8.82064077e+00  7.83241044e+00]\n",
      " [-4.33821107e-02  9.32046372e+00  7.11158756e+00]]\n",
      "sqP = \n",
      "[[1.5727633772164402, 0.0, 0.0], [0.0001919879638604272, 0.5910848433822876, 0.0], [0.009955079883921759, 0.06707919549625393, 0.5701227471960132]]\n",
      "m = \n",
      "[-0.01273801  9.40948781  7.61712356]\n",
      "P = \n",
      "[[2.47352748e+00 3.11294787e-04 1.23762892e-02]\n",
      " [3.11294787e-04 1.34361585e-01 4.41961195e-02]\n",
      " [1.23762892e-02 4.41961195e-02 1.41257929e-01]]\n",
      "num = \n",
      "836\n",
      "SENSOR 9 4.86345125952 -2.16850372828\n",
      "\n",
      "num = \n",
      "837\n",
      "SENSOR 5 2.53715647378 -0.0219189488566\n",
      "\n",
      "num = \n",
      "838\n",
      "SENSOR 6 0.638025009019 -2.44082398988\n",
      "\n",
      "num = \n",
      "839\n",
      "ODOMETRY -0.0110796247981 0.100123354178 -0.0203009558875\n",
      "\n",
      "sqP = \n",
      "[[1.5727452062980771, 0.0, 0.0], [0.00019793084445470833, 0.36655360629114125, 0.0], [0.00786922710171497, 0.12056780020800383, 0.3558924138340017]]\n",
      "sigma = \n",
      "[[-1.27380111e-02  9.40948781e+00  7.61712356e+00]\n",
      " [ 3.13275240e+00  9.40948781e+00  7.61712356e+00]\n",
      " [-1.23421494e-02  1.01425950e+01  7.61712356e+00]\n",
      " [ 3.00044312e-03  9.65062341e+00  8.32890839e+00]\n",
      " [-3.15822842e+00  9.40948781e+00  7.61712356e+00]\n",
      " [-1.31338728e-02  8.67638060e+00  7.61712356e+00]\n",
      " [-2.84764653e-02  9.16835221e+00  6.90533873e+00]]\n",
      "sigmax = \n",
      "[[-0.0432707   9.5094375   7.61481157]\n",
      " [ 3.10221972  9.30952987  7.61904595]\n",
      " [-0.04287483 10.24254562  7.61485114]\n",
      " [-0.02753224  9.7505971   8.32816968]\n",
      " [ 3.0944242   9.30954789  7.61982511]\n",
      " [-0.04366656  8.77632936  7.61477201]\n",
      " [-0.05900915  9.26825313  6.90145404]]\n",
      "sqP = \n",
      "[[1.4319974117460446, 0.0, 0.0], [-0.08151279330412249, 0.5913971732533122, 0.0], [0.0038618039330771798, 0.07297839369529259, 0.567347451875776]]\n",
      "m = \n",
      "[0.73350287 9.50842781 7.53953789]\n",
      "P = \n",
      "[[ 2.04648924e+00 -8.78504673e-02 -4.77112449e-04]\n",
      " [-8.78504673e-02  1.48757001e-01  5.33073924e-02]\n",
      " [-4.77112449e-04  5.33073924e-02  1.41167805e-01]]\n",
      "num = \n",
      "840\n",
      "SENSOR 9 4.80447773758 -2.11401632864\n",
      "\n",
      "num = \n",
      "841\n",
      "SENSOR 5 2.52598129404 -0.147975261228\n",
      "\n",
      "num = \n",
      "842\n",
      "SENSOR 6 0.774158398614 -2.52392662998\n",
      "\n",
      "num = \n",
      "843\n",
      "ODOMETRY -0.0105396114623 0.100008052618 -0.0201317969487\n",
      "\n",
      "sqP = \n",
      "[[1.4305555699984414, 0.0, 0.0], [-0.06141003475554482, 0.38077002040069863, 0.0], [-0.00033351549466544093, 0.13994513303347167, 0.3486876153790151]]\n",
      "sigma = \n",
      "[[ 0.73350287  9.50842781  7.53953789]\n",
      " [ 3.59461401  9.50842781  7.53953789]\n",
      " [ 0.6106828  10.26996785  7.53953789]\n",
      " [ 0.73283584  9.78831808  8.23691312]\n",
      " [-2.12760827  9.50842781  7.53953789]\n",
      " [ 0.85632294  8.74688777  7.53953789]\n",
      " [ 0.7341699   9.22853754  6.84216265]]\n",
      "sigmax = \n",
      "[[ 0.70212229  9.58354092  7.6057399 ]\n",
      " [-2.71995188  9.41792402  7.49671558]\n",
      " [ 0.57930222 10.35262565  7.59603899]\n",
      " [ 0.70145526  9.86347533  8.30306502]\n",
      " [-2.15898885  9.45457589  7.45513021]\n",
      " [ 0.82494236  8.81332455  7.61444343]\n",
      " [ 0.70278932  9.30360648  6.90841476]]\n",
      "sqP = \n",
      "[[1.4401352709632382, 0.0, 0.0], [0.04461847904288031, 0.6081380183036533, 0.0], [0.05213321901984479, 0.07923426988803527, 0.5620059475939322]]\n",
      "m = \n",
      "[-0.09192156  9.50262055  7.50739117]\n",
      "P = \n",
      "[[2.07160764 0.04813825 0.06052672]\n",
      " [0.04813825 0.14418419 0.0575877 ]\n",
      " [0.06052672 0.0575877  0.14198381]]\n",
      "num = \n",
      "844\n",
      "SENSOR 9 4.92716774584 -2.183105981\n",
      "\n",
      "num = \n",
      "845\n",
      "SENSOR 5 2.5239692479 -0.10125627604\n",
      "\n",
      "num = \n",
      "846\n",
      "SENSOR 6 0.898459257994 -2.5942905006\n",
      "\n",
      "num = \n",
      "847\n",
      "ODOMETRY -0.0101753448007 0.100002913472 -0.0206574320107\n",
      "\n",
      "sqP = \n",
      "[[1.4393080405257397, 0.0, 0.0], [0.03344541319227655, 0.37824012326261225, 0.0], [0.04205265249006397, 0.14853323848572203, 0.3437342851071983]]\n",
      "sigma = \n",
      "[[-9.19215639e-02  9.50262055e+00  7.50739117e+00]\n",
      " [ 2.78669452e+00  9.50262055e+00  7.50739117e+00]\n",
      " [-2.50307375e-02  1.02591008e+01  7.50739117e+00]\n",
      " [-7.81625894e-03  9.79968703e+00  8.19485974e+00]\n",
      " [-2.97053764e+00  9.50262055e+00  7.50739117e+00]\n",
      " [-1.58812390e-01  8.74614030e+00  7.50739117e+00]\n",
      " [-1.76026869e-01  9.20555407e+00  6.81992260e+00]]\n",
      "sigmax = \n",
      "[[-0.12259297  9.60210411  7.49716215]\n",
      " [ 2.75602311  9.40921629  7.54312987]\n",
      " [-0.05570215 10.35904559  7.5038346 ]\n",
      " [-0.03848767  9.89967823  8.19302411]\n",
      " [-3.00120905  9.4038981   7.49140719]\n",
      " [-0.1894838   8.84471766  7.49053545]\n",
      " [-0.20669828  9.30382668  6.8013725 ]]\n",
      "sqP = \n",
      "[[1.508142783945571, 0.0, 0.0], [0.01381815051916343, 0.6103120607063647, 0.0], [0.022115331224666423, 0.08530169653525062, 0.5599391291509813]]\n",
      "m = \n",
      "[-0.12284903  9.58643181  7.47414789]\n",
      "P = \n",
      "[[2.27415219 0.01591105 0.02694815]\n",
      " [0.01591105 0.14452965 0.05893261]\n",
      " [0.02694815 0.05893261 0.13948931]]\n",
      "num = \n",
      "848\n",
      "SENSOR 9 5.01194140521 -2.08598842578\n",
      "\n",
      "num = \n",
      "849\n",
      "SENSOR 5 2.42112473613 -0.0714350873771\n",
      "\n",
      "num = \n",
      "850\n",
      "SENSOR 6 0.878381860102 -2.66964579914\n",
      "\n",
      "num = \n",
      "851\n",
      "ODOMETRY -0.0103462997287 0.0999423849927 -0.0195755267881\n",
      "\n",
      "sqP = \n",
      "[[1.5080292399798536, 0.0, 0.0], [0.010550887399012549, 0.38002411335887265, 0.0], [0.017869782050777234, 0.154579838681398, 0.33952180649424873]]\n",
      "sigma = \n",
      "[[-0.12284903  9.58643181  7.47414789]\n",
      " [ 2.89320945  9.58643181  7.47414789]\n",
      " [-0.10174725 10.34648004  7.47414789]\n",
      " [-0.08710946  9.89559149  8.1531915 ]\n",
      " [-3.13890751  9.58643181  7.47414789]\n",
      " [-0.1439508   8.82638359  7.47414789]\n",
      " [-0.15858859  9.27727214  6.79510427]]\n",
      "sigmax = \n",
      "[[-0.1536818   9.68555123  7.46088426]\n",
      " [ 2.86237668  9.48975304  7.49971736]\n",
      " [-0.13258003 10.44585726  7.46297865]\n",
      " [-0.11794224  9.99512155  8.14347807]\n",
      " [ 3.11344503  9.48643171  7.47489692]\n",
      " [-0.17478358  8.92520107  7.45879577]\n",
      " [-0.18942137  9.37585432  6.77830739]]\n",
      "sqP = \n",
      "[[1.4334996277386858, 0.0, 0.0], [-0.07643574616456682, 0.6082431506305177, 0.0], [0.014846533430789618, 0.08778949381655196, 0.555629137888905]]\n",
      "m = \n",
      "[0.62889467 9.64030781 7.41808701]\n",
      "P = \n",
      "[[ 2.0508558  -0.08050193  0.01242761]\n",
      " [-0.08050193  0.14591625  0.05612175]\n",
      " [ 0.01242761  0.05612175  0.13694479]]\n",
      "num = \n",
      "852\n",
      "SENSOR 5 2.23964032564 -0.0586867747141\n",
      "\n",
      "num = \n",
      "853\n",
      "SENSOR 6 1.24988717446 -2.58043089475\n",
      "\n",
      "num = \n",
      "854\n",
      "ODOMETRY -0.0107865547639 0.10003293752 -0.0196069887085\n",
      "\n",
      "sqP = \n",
      "[[1.4320809346511603, 0.0, 0.0], [-0.05621325309144584, 0.37783106966959173, 0.0], [0.008678007002216642, 0.14982771996494654, 0.338261939849259]]\n",
      "sigma = \n",
      "[[ 0.62889467  9.64030781  7.41808701]\n",
      " [ 3.49305654  9.64030781  7.41808701]\n",
      " [ 0.51646816 10.39596995  7.41808701]\n",
      " [ 0.64625068  9.93996325  8.09461089]\n",
      " [-2.2352672   9.64030781  7.41808701]\n",
      " [ 0.74132117  8.88464567  7.41808701]\n",
      " [ 0.61153865  9.34065237  6.74156313]]\n",
      "sigmax = \n",
      "[[ 0.59897284  9.72173297  7.47603891]\n",
      " [-2.8200506   9.54612397  7.38465224]\n",
      " [ 0.48654634 10.48338267  7.46653798]\n",
      " [ 0.61632886 10.02037039  8.15396721]\n",
      " [-2.26518903  9.57786835  7.34004988]\n",
      " [ 0.71139935  8.95905517  7.48480812]\n",
      " [ 0.58161683  9.42307103  6.79809316]]\n",
      "sqP = \n",
      "[[1.4398013831256375, 0.0, 0.0], [0.052065694046493805, 0.6079861927455834, 0.0], [0.04760774170323474, 0.08175542759851215, 0.5554390416346517]]\n",
      "m = \n",
      "[-0.19759079  9.71029669  7.26125948]\n",
      "P = \n",
      "[[2.07173394 0.06968622 0.05702703]\n",
      " [0.06968622 0.29454029 0.07114321]\n",
      " [0.05702703 0.07114321 0.13769676]]\n",
      "num = \n",
      "855\n",
      "SENSOR 5 1.94979000746 -0.0453900849002\n",
      "\n",
      "num = \n",
      "856\n",
      "SENSOR 6 1.290168152 -2.6828250479\n",
      "\n",
      "num = \n",
      "857\n",
      "ODOMETRY -0.0108544957486 0.100230199128 -0.0189367396772\n",
      "\n",
      "sqP = \n",
      "[[1.439351915240442, 0.0, 0.0], [0.04841499678169106, 0.5405518238893474, 0.0], [0.03961993851705822, 0.1280635954344262, 0.3460155109005536]]\n",
      "sigma = \n",
      "[[-0.19759079  9.71029669  7.26125948]\n",
      " [ 2.68111304  9.71029669  7.26125948]\n",
      " [-0.1007608  10.79140034  7.26125948]\n",
      " [-0.11835092  9.96642388  7.9532905 ]\n",
      " [-3.07629462  9.71029669  7.26125948]\n",
      " [-0.29442079  8.62919304  7.26125948]\n",
      " [-0.27683067  9.4541695   6.56922846]]\n",
      "sigmax = \n",
      "[[-0.22798434  9.80816571  7.24056541]\n",
      " [ 2.65071949  9.62116793  7.30667592]\n",
      " [-0.13115434 10.89081158  7.2501242 ]\n",
      " [-0.14874446 10.06562388  7.94040838]\n",
      " [-3.10668817  9.61041234  7.25580924]\n",
      " [-0.32481433  8.72460293  7.23120049]\n",
      " [-0.30722421  9.55009334  6.5408523 ]]\n",
      "sqP = \n",
      "[[1.5085250038855158, 0.0, 0.0], [0.0233312922526414, 0.7185214383385968, 0.0], [0.021474878673757872, 0.06363966185061969, 0.5642735737083965]]\n",
      "m = \n",
      "[-0.23746015  9.70256401  7.02938876]\n",
      "P = \n",
      "[[2.27539932 0.03300138 0.02724607]\n",
      " [0.03300138 0.40227074 0.09056542]\n",
      " [0.02724607 0.09056542 0.13134871]]\n",
      "num = \n",
      "858\n",
      "SENSOR 5 2.22911731184 -0.00122688508787\n",
      "\n",
      "num = \n",
      "859\n",
      "SENSOR 6 1.21205806069 -2.64632589698\n",
      "\n",
      "num = \n",
      "860\n",
      "ODOMETRY -0.00993341143727 0.0999418232338 -0.0202662925326\n",
      "\n",
      "sqP = \n",
      "[[1.5084426796187376, 0.0, 0.0], [0.021877785044486085, 0.6338707314296593, 0.0], [0.018062383288578288, 0.142253380474553, 0.33284596207598094]]\n",
      "sigma = \n",
      "[[-0.23746015  9.70256401  7.02938876]\n",
      " [ 2.77942521  9.70256401  7.02938876]\n",
      " [-0.19370458 10.97030548  7.02938876]\n",
      " [-0.20133538  9.98707078  7.69508069]\n",
      " [-3.25434551  9.70256401  7.02938876]\n",
      " [-0.28121572  8.43482255  7.02938876]\n",
      " [-0.27358492  9.41805725  6.36369684]]\n",
      "sigmax = \n",
      "[[-0.26725138  9.79971995  7.00475512]\n",
      " [ 2.74963397  9.60922661  7.06591577]\n",
      " [-0.22349581 11.06844594  7.00902845]\n",
      " [-0.23112662 10.08505302  7.67397209]\n",
      " [ 2.99904856  9.60309854  7.04174643]\n",
      " [-0.31100695  8.53080799  7.00052894]\n",
      " [-0.30337615  9.51426011  6.3355703 ]]\n",
      "sqP = \n",
      "[[1.4336121127030628, 0.0, 0.0], [-0.06809553531738756, 0.7907112827146009, 0.0], [0.02414443933434253, 0.061929319952430145, 0.5550059746287421]]\n",
      "m = \n",
      "[0.53395155 9.62340849 7.10724942]\n",
      "P = \n",
      "[[ 2.05329763 -0.08466439  0.01898459]\n",
      " [-0.08466439  0.52025021  0.11357129]\n",
      " [ 0.01898459  0.11357129  0.12560104]]\n",
      "num = \n",
      "861\n",
      "SENSOR 4 4.99272912579 -0.274166164371\n",
      "\n",
      "num = \n",
      "862\n",
      "SENSOR 5 1.81092330137 0.0761110639692\n",
      "\n",
      "num = \n",
      "863\n",
      "SENSOR 6 1.36882473059 -2.60089763233\n",
      "\n",
      "num = \n",
      "864\n",
      "ODOMETRY -0.0104826202942 0.0999751788151 -0.0191967893743\n",
      "\n",
      "sqP = \n",
      "[[1.4329332274806523, 0.0, 0.0], [-0.05908467335651788, 0.7188596595846655, 0.0], [0.013248760871998545, 0.15907707507665655, 0.31641744302696034]]\n",
      "sigma = \n",
      "[[ 0.53395155  9.62340849  7.10724942]\n",
      " [ 3.399818    9.62340849  7.10724942]\n",
      " [ 0.4157822  11.06112781  7.10724942]\n",
      " [ 0.56044907  9.94156264  7.74008431]\n",
      " [-2.33191491  9.62340849  7.10724942]\n",
      " [ 0.65212089  8.18568917  7.10724942]\n",
      " [ 0.50745402  9.30525434  6.47441454]]\n",
      "sigmax = \n",
      "[[ 0.50375184  9.70993968  7.15725663]\n",
      " [-2.91356701  9.52653152  7.08268886]\n",
      " [ 0.3855825  11.15295112  7.14670633]\n",
      " [ 0.53024936 10.02673854  7.79236655]\n",
      " [-2.36211461  9.55376002  7.03557335]\n",
      " [ 0.62192119  8.26572133  7.16710944]\n",
      " [ 0.47725432  9.39308007  6.52211159]]\n",
      "sqP = \n",
      "[[1.4398692953345014, 0.0, 0.0], [0.04186623606664946, 0.8659024026500163, 0.0], [0.04209660759968458, 0.05531636870350968, 0.5458757723853501]]\n",
      "m = \n",
      "[-0.29758291  9.67572797  6.90633987]\n",
      "P = \n",
      "[[2.07173107 0.05209829 0.04542572]\n",
      " [0.05209829 0.55333797 0.06903844]\n",
      " [0.04542572 0.06903844 0.07953316]]\n",
      "num = \n",
      "865\n",
      "SENSOR 4 4.69854535708 -0.223485928377\n",
      "\n",
      "num = \n",
      "866\n",
      "SENSOR 5 1.7001461128 0.0429298228702\n",
      "\n",
      "num = \n",
      "867\n",
      "SENSOR 6 1.30623051674 -2.58399330818\n",
      "\n",
      "num = \n",
      "868\n",
      "ODOMETRY 0.000324810885789 0.0998821776257 -0.000359989641411\n",
      "\n",
      "sqP = \n",
      "[[1.4393509202080688, 0.0, 0.0], [0.03619567985748252, 0.7429857651979422, 0.0], [0.0315598670632494, 0.09138278245604424, 0.2649270112401264]]\n",
      "sigma = \n",
      "[[-0.29758291  9.67572797  6.90633987]\n",
      " [ 2.58111893  9.67572797  6.90633987]\n",
      " [-0.22519155 11.1616995   6.90633987]\n",
      " [-0.23446317  9.85849353  7.43619389]\n",
      " [-3.17628475  9.67572797  6.90633987]\n",
      " [-0.36997427  8.18975644  6.90633987]\n",
      " [-0.36070264  9.4929624   6.37648585]]\n",
      "sigmax = \n",
      "[[-0.32726232  9.77099651  6.87602582]\n",
      " [ 2.55143953  9.5916104   6.96037015]\n",
      " [-0.25487096 11.25891108  6.88299581]\n",
      " [-0.26414258  9.9554845   7.41194954]\n",
      " [ 3.07722115  9.57585478  6.91085468]\n",
      " [-0.39965368  8.2825829   6.86921461]\n",
      " [-0.39038205  9.58612908  6.34022283]]\n",
      "sqP = \n",
      "[[1.4387963276872882, 0.0, 0.0], [-0.05622220400306563, 0.8749880404039713, 0.0], [0.029208728181533913, 0.030750238680789528, 0.5204022041919992]]\n",
      "m = \n",
      "[0.46220422 9.52552537 6.74595494]\n",
      "P = \n",
      "[[ 2.06828045 -0.06523277  0.02787052]\n",
      " [-0.06523277  0.55478082  0.05713787]\n",
      " [ 0.02787052  0.05713787  0.0705807 ]]\n",
      "num = \n",
      "869\n",
      "SENSOR 4 5.09005860279 -0.233743736727\n",
      "\n",
      "num = \n",
      "870\n",
      "SENSOR 5 1.58907104446 0.12166808225\n",
      "\n",
      "num = \n",
      "871\n",
      "SENSOR 6 1.50018263441 -2.63967144455\n",
      "\n",
      "num = \n",
      "872\n",
      "ODOMETRY -0.000418164888098 0.0999656435967 -3.68260681419e-05\n",
      "\n",
      "sqP = \n",
      "[[1.4381517495066076, 0.0, 0.0], [-0.04535875250064004, 0.7434536996056084, 0.0], [0.01937940276262551, 0.07803699498848876, 0.25321011533884247]]\n",
      "sigma = \n",
      "[[ 0.46220422  9.52552537  6.74595494]\n",
      " [ 3.33850772  9.52552537  6.74595494]\n",
      " [ 0.37148671 11.01243277  6.74595494]\n",
      " [ 0.50096302  9.68159936  7.25237517]\n",
      " [-2.41409928  9.52552537  6.74595494]\n",
      " [ 0.55292172  8.03861797  6.74595494]\n",
      " [ 0.42344541  9.36945138  6.2395347 ]]\n",
      "sigmax = \n",
      "[[ 0.46216904  9.61491261  6.79052364]\n",
      " [-2.94471277  9.42757979  6.72638168]\n",
      " [ 0.37145153 11.10549007  6.78224251]\n",
      " [ 0.50092784  9.76919247  7.30037408]\n",
      " [-2.41413446  9.45095057  6.67950909]\n",
      " [ 0.55288654  8.12360003  6.79843825]\n",
      " [ 0.42341023  9.46049849  6.28060627]]\n",
      "sqP = \n",
      "[[1.4389504462517442, 0.0, 0.0], [0.049922776378401174, 0.8746439826912781, 0.0], [0.03830237858713962, 0.020071379476763827, 0.5145045429221128]]\n",
      "m = \n",
      "[-0.30980823  9.80513726  6.76927678]\n",
      "P = \n",
      "[[2.06932122 0.06490054 0.04318913]\n",
      " [0.06490054 0.59180872 0.06228048]\n",
      " [0.04318913 0.06228048 0.06760855]]\n",
      "num = \n",
      "873\n",
      "SENSOR 4 4.74946984204 -0.126245943621\n",
      "\n",
      "num = \n",
      "874\n",
      "SENSOR 5 1.55519072464 0.0805634344737\n",
      "\n",
      "num = \n",
      "875\n",
      "SENSOR 6 1.63437464886 -2.74620564163\n",
      "\n",
      "num = \n",
      "876\n",
      "ODOMETRY 0.000475115856201 0.100024313407 0.000393598981251\n",
      "\n",
      "sqP = \n",
      "[[1.4385135445420896, 0.0, 0.0], [0.045116389436652304, 0.7679669445776283, 0.0], [0.03002344158567996, 0.07933405737606315, 0.2457910739125191]]\n",
      "sigma = \n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[-0.30980823  9.80513726  6.76927678]\n",
      " [ 2.56721886  9.80513726  6.76927678]\n",
      " [-0.21957545 11.34107115  6.76927678]\n",
      " [-0.24976135  9.96380538  7.26085892]\n",
      " [-3.18683532  9.80513726  6.76927678]\n",
      " [-0.40004101  8.26920337  6.76927678]\n",
      " [-0.36985511  9.64646915  6.27769463]]\n",
      "sigmax = \n",
      "[[-0.31026322  9.90033099  6.73875984]\n",
      " [ 2.56676387  9.72123556  6.82362412]\n",
      " [-0.22003044 11.43862751  6.74746193]\n",
      " [-0.25021634 10.06065889  7.23610964]\n",
      " [ 3.095895    9.70527581  6.77383971]\n",
      " [-0.400496    8.36125994  6.73030605]\n",
      " [-0.3703101   9.73965997  6.24152004]]\n",
      "sqP = \n",
      "[[1.439092233084887, 0.0, 0.0], [-0.05107054597555508, 0.8957028583139269, 0.0], [0.028701174782329156, 0.025309263833485644, 0.5110846753990831]]\n",
      "m = \n",
      "[0.4710109  9.79782247 6.63012712]\n",
      "P = \n",
      "[[ 2.06925226 -0.05667828  0.0281581 ]\n",
      " [-0.05667828  0.54138051  0.05159077]\n",
      " [ 0.0281581   0.05159077  0.06916935]]\n",
      "num = \n",
      "877\n",
      "SENSOR 4 4.50322759327 -0.208257404441\n",
      "\n",
      "num = \n",
      "878\n",
      "SENSOR 5 1.40431609203 0.102953443634\n",
      "\n",
      "num = \n",
      "879\n",
      "SENSOR 6 1.92474673868 -2.70532942045\n",
      "\n",
      "num = \n",
      "880\n",
      "SENSOR 7 4.8035115893 -1.34951568347\n",
      "\n",
      "num = \n",
      "881\n",
      "ODOMETRY -8.36142030851e-05 0.0998721448335 -0.000260661559938\n",
      "\n",
      "sqP = \n",
      "[[1.4384895768404906, 0.0, 0.0], [-0.03940124178796948, 0.7347299186714124, 0.0], [0.019574768518239406, 0.0712670543867837, 0.2524028254852986]]\n",
      "sigma = \n",
      "[[ 0.4710109   9.79782247  6.63012712]\n",
      " [ 3.34799006  9.79782247  6.63012712]\n",
      " [ 0.39220842 11.26728231  6.63012712]\n",
      " [ 0.51016044  9.94035658  7.13493277]\n",
      " [-2.40596825  9.79782247  6.63012712]\n",
      " [ 0.54981339  8.32836263  6.63012712]\n",
      " [ 0.43186137  9.65528836  6.12532147]]\n",
      "sigmax = \n",
      "[[ 0.47187962  9.88693356  6.67555923]\n",
      " [-2.93432654  9.69993087  6.60958211]\n",
      " [ 0.39307713 11.35969332  6.66840333]\n",
      " [ 0.51102915 10.0276212   7.18381783]\n",
      " [-2.40509954  9.72369515  6.56297055]\n",
      " [ 0.5506821   8.41362073  6.68243315]\n",
      " [ 0.43273008  9.74610936  6.167231  ]]\n",
      "sqP = \n",
      "[[1.438746513707237, 0.0, 0.0], [0.05298414982619092, 0.8662462593889466, 0.0], [0.03901225996437905, 0.018579626084492255, 0.5141635617901422]]\n",
      "m = \n",
      "[-0.34449061  9.65189981  6.39147896]\n",
      "P = \n",
      "[[2.06655869 0.03899338 0.04044689]\n",
      " [0.03899338 0.15855589 0.00639946]\n",
      " [0.04044689 0.00639946 0.0631167 ]]\n",
      "num = \n",
      "882\n",
      "SENSOR 4 4.36260790638 -0.247754318384\n",
      "\n",
      "num = \n",
      "883\n",
      "SENSOR 5 1.47019886492 0.0682113654816\n",
      "\n",
      "num = \n",
      "884\n",
      "SENSOR 6 1.77940808395 -2.71217766237\n",
      "\n",
      "num = \n",
      "885\n",
      "SENSOR 7 5.2180857839 -1.31953374687\n",
      "\n",
      "num = \n",
      "886\n",
      "ODOMETRY 0.000197959165217 0.100131767346 0.000281610671693\n",
      "\n",
      "sqP = \n",
      "[[1.4375530231479323, 0.0, 0.0], [0.027124829639877265, 0.39726582069930627, 0.0], [0.02813593045154869, 0.014187661219699032, 0.24924641434927594]]\n",
      "sigma = \n",
      "[[-0.34449061  9.65189981  6.39147896]\n",
      " [ 2.53061544  9.65189981  6.39147896]\n",
      " [-0.29024095 10.44643145  6.39147896]\n",
      " [-0.28821875  9.68027513  6.88997179]\n",
      " [-3.21959666  9.65189981  6.39147896]\n",
      " [-0.39874027  8.85736817  6.39147896]\n",
      " [-0.40076247  9.62352449  5.89298613]]\n",
      "sigmax = \n",
      "[[-0.34483489  9.74590141  6.35774256]\n",
      " [ 2.53027116  9.57010054  6.44877927]\n",
      " [-0.29058523 10.54212405  6.36288924]\n",
      " [-0.28856303  9.77602535  6.86157564]\n",
      " [ 3.06324437  9.552332    6.39926982]\n",
      " [-0.39908455  8.94940218  6.35269513]\n",
      " [-0.40110675  9.71547988  5.85401627]]\n",
      "sqP = \n",
      "[[1.4386910683452439, 0.0, 0.0], [-0.06818530289240829, 0.6004465256099883, 0.0], [0.030951861612560083, 0.007827587314488932, 0.5132365814097388]]\n",
      "m = \n",
      "[0.42389592 9.85054802 6.38187147]\n",
      "P = \n",
      "[[ 2.06601360e+00 -7.12967952e-02  3.20387038e-02]\n",
      " [-7.12967952e-02  1.28004278e-01  4.46950867e-04]\n",
      " [ 3.20387038e-02  4.46950867e-04  5.91609772e-02]]\n",
      "num = \n",
      "887\n",
      "SENSOR 4 4.44601043546 -0.326392394425\n",
      "\n",
      "num = \n",
      "888\n",
      "SENSOR 5 1.19003310152 0.115920050334\n",
      "\n",
      "num = \n",
      "889\n",
      "SENSOR 6 2.05288544044 -2.83359564739\n",
      "\n",
      "num = \n",
      "890\n",
      "SENSOR 7 4.97368047041 -1.40535275082\n",
      "\n",
      "num = \n",
      "891\n",
      "ODOMETRY -0.000164122759813 0.0999543664159 -0.000303793093368\n",
      "\n",
      "sqP = \n",
      "[[1.4373634180902564, 0.0, 0.0], [-0.04960248350557509, 0.35432170588048595, 0.0], [0.02228991176092274, 0.004381853614717947, 0.24216716631387672]]\n",
      "sigma = \n",
      "[[ 0.42389592  9.85054802  6.38187147]\n",
      " [ 3.29862276  9.85054802  6.38187147]\n",
      " [ 0.32469095 10.55919143  6.38187147]\n",
      " [ 0.46847574  9.85931173  6.86620581]\n",
      " [-2.45083092  9.85054802  6.38187147]\n",
      " [ 0.52310089  9.14190461  6.38187147]\n",
      " [ 0.3793161   9.84178431  5.89753714]]\n",
      "sigmax = \n",
      "[[ 0.42437549  9.94180931  6.4230752 ]\n",
      " [-2.98408298  9.75165137  6.36619273]\n",
      " [ 0.32517052 10.65408493  6.41383388]\n",
      " [ 0.46895531  9.9486461   6.91143567]\n",
      " [-2.45035135  9.773383    6.3180598 ]\n",
      " [ 0.52358046  9.22863627  6.43191135]\n",
      " [ 0.37979567  9.93479118  5.93463287]]\n",
      "sqP = \n",
      "[[1.4392054797234013, 0.0, 0.0], [0.06158367403433824, 0.573810259961738, 0.0], [0.035920374231220895, -0.0005687056277469274, 0.5096418230535051]]\n",
      "m = \n",
      "[-0.37627652  9.85547907  6.24961297]\n",
      "P = \n",
      "[[2.06784300e+00 6.56655236e-02 3.74675289e-02]\n",
      " [6.56655236e-02 1.19325800e-01 1.48640165e-03]\n",
      " [3.74675289e-02 1.48640165e-03 6.00886604e-02]]\n",
      "num = \n",
      "892\n",
      "SENSOR 4 4.42845729357 -0.199738979168\n",
      "\n",
      "num = \n",
      "893\n",
      "SENSOR 5 1.17917386982 0.18688342145\n",
      "\n",
      "num = \n",
      "894\n",
      "SENSOR 6 1.9596965856 -2.82416076788\n",
      "\n",
      "num = \n",
      "895\n",
      "SENSOR 7 4.92694129534 -1.35478197562\n",
      "\n",
      "num = \n",
      "896\n",
      "ODOMETRY 0.000333398378648 0.0999246910299 -0.000238132530679\n",
      "\n",
      "sqP = \n",
      "[[1.4379996513942188, 0.0, 0.0], [0.045664491984693086, 0.3424040801261148, 0.0], [0.026055311526155533, 0.0008662253145522105, 0.24373967837879076]]\n",
      "sigma = \n",
      "[[-0.37627652  9.85547907  6.24961297]\n",
      " [ 2.49972278  9.85547907  6.24961297]\n",
      " [-0.28494754 10.54028723  6.24961297]\n",
      " [-0.3241659   9.85721152  6.73709233]\n",
      " [-3.25227583  9.85547907  6.24961297]\n",
      " [-0.46760551  9.17067091  6.24961297]\n",
      " [-0.42838715  9.85374662  5.76213362]]\n",
      "sigmax = \n",
      "[[-0.37674444  9.94843453  6.21286848]\n",
      " [ 2.49925486  9.77542768  6.30946821]\n",
      " [-0.28541545 10.63620647  6.22149935]\n",
      " [-0.32463382  9.95195471  6.70523949]\n",
      " [ 3.03044157  9.75613815  6.26066997]\n",
      " [-0.46807342  9.2598878   6.20454389]\n",
      " [-0.42885506  9.94466199  5.72059723]]\n",
      "sqP = \n",
      "[[1.4390795347334604, 0.0, 0.0], [-0.06418721742659424, 0.5661634034589246, 0.0], [0.032991298699870004, 0.0028537899209497924, 0.5105299670430566]]\n",
      "m = \n",
      "[0.41869187 9.8101513  6.22410605]\n",
      "P = \n",
      "[[ 2.06746055 -0.06907035  0.03448563]\n",
      " [-0.06907035  0.11823344 -0.00208028]\n",
      " [ 0.03448563 -0.00208028  0.05990869]]\n",
      "num = \n",
      "897\n",
      "SENSOR 4 4.10993462238 -0.218612334131\n",
      "\n",
      "num = \n",
      "898\n",
      "SENSOR 5 1.02261696425 0.152968841424\n",
      "\n",
      "num = \n",
      "899\n",
      "SENSOR 6 2.17175440342 -2.77569049944\n",
      "\n",
      "num = \n",
      "900\n",
      "SENSOR 7 4.9569182734 -1.483064888\n",
      "\n",
      "num = \n",
      "901\n",
      "ODOMETRY 0.000655493141672 0.100066766391 0.000616803666521\n",
      "\n",
      "sqP = \n",
      "[[1.4378666670005495, 0.0, 0.0], [-0.04803668730754407, 0.34047894648063803, 0.0], [0.023983886234942773, -0.0027260737755532135, 0.2435693472240858]]\n",
      "sigma = \n",
      "[[ 0.41869187  9.8101513   6.22410605]\n",
      " [ 3.29442521  9.8101513   6.22410605]\n",
      " [ 0.3226185  10.49110919  6.22410605]\n",
      " [ 0.46665965  9.80469915  6.71124474]\n",
      " [-2.45704146  9.8101513   6.22410605]\n",
      " [ 0.51476525  9.1291934   6.22410605]\n",
      " [ 0.3707241   9.81560344  5.73696736]]\n",
      "sigmax = \n",
      "[[ 0.41878714  9.9014311   6.26476243]\n",
      " [-2.98866483  9.71139642  6.20886076]\n",
      " [ 0.32271377 10.58586805  6.25581887]\n",
      " [ 0.46675491  9.89392451  6.75623117]\n",
      " [-2.45694619  9.73276042  6.16089533]\n",
      " [ 0.51486051  9.21615228  6.27333102]\n",
      " [ 0.37081937  9.9087277   5.77320016]]\n",
      "sqP = \n",
      "[[1.4391330271325542, 0.0, 0.0], [0.06234048307958703, 0.5651841200886684, 0.0], [0.03579955351673175, -0.0035799126110842, 0.5103524368792135]]\n",
      "m = \n",
      "[-0.38352519  9.83273554  6.04313829]\n",
      "P = \n",
      "[[2.06763091 0.06729751 0.037418  ]\n",
      " [0.06729751 0.11874586 0.00226664]\n",
      " [0.037418   0.00226664 0.05970599]]\n",
      "num = \n",
      "902\n",
      "SENSOR 4 3.89656801687 -0.154099643488\n",
      "\n",
      "num = \n",
      "903\n",
      "SENSOR 5 0.905689372217 0.172402059922\n",
      "\n",
      "num = \n",
      "904\n",
      "SENSOR 6 2.13167650164 -2.75783273628\n",
      "\n",
      "num = \n",
      "905\n",
      "SENSOR 7 4.8549234685 -1.39334612816\n",
      "\n",
      "num = \n",
      "906\n",
      "ODOMETRY -0.000159452291233 0.100045036471 0.000808106642051\n",
      "\n",
      "sqP = \n",
      "[[1.4379259067657582, 0.0, 0.0], [0.04680179087317596, 0.3414021884960458, 0.0], [0.026022205171766313, 0.0030719050430511406, 0.2429390738703449]]\n",
      "sigma = \n",
      "[[-0.38352519  9.83273554  6.04313829]\n",
      " [ 2.49232662  9.83273554  6.04313829]\n",
      " [-0.28992161 10.51553992  6.04313829]\n",
      " [-0.33148078  9.83887935  6.52901644]\n",
      " [-3.259377    9.83273554  6.04313829]\n",
      " [-0.47712877  9.14993116  6.04313829]\n",
      " [-0.4355696   9.82659173  5.55726014]]\n",
      "sigmax = \n",
      "[[-0.38225289  9.92555711  6.00575495]\n",
      " [ 2.49359892  9.75298994  6.10358659]\n",
      " [-0.28864931 10.61144926  6.01459435]\n",
      " [-0.33020848  9.93351995  6.49651238]\n",
      " [ 3.0250806   9.73335441  6.05483221]\n",
      " [-0.47585648  9.23885229  5.99724285]\n",
      " [-0.4342973   9.9173429   5.51509876]]\n",
      "sqP = \n",
      "[[1.4391222799032444, 0.0, 0.0], [-0.06380009652950827, 0.5656380037476662, 0.0], [0.03348869264901095, 0.003836668333991753, 0.5101364169550179]]\n",
      "m = \n",
      "[0.4093847  9.75520487 5.90661123]\n",
      "P = \n",
      "[[ 2.06758708e+00 -6.86849364e-02  3.50081091e-02]\n",
      " [-6.86849364e-02  1.18251150e-01 -1.13573756e-03]\n",
      " [ 3.50081091e-02 -1.13573756e-03  6.10875828e-02]]\n",
      "num = \n",
      "907\n",
      "SENSOR 4 3.94173958574 -0.302887305467\n",
      "\n",
      "num = \n",
      "908\n",
      "SENSOR 5 1.14270798286 0.168760719018\n",
      "\n",
      "num = \n",
      "909\n",
      "SENSOR 6 2.18631073434 -2.81520889899\n",
      "\n",
      "num = \n",
      "910\n",
      "SENSOR 7 5.10636542466 -1.4209085397\n",
      "\n",
      "num = \n",
      "911\n",
      "ODOMETRY 0.00078211331779 0.10000393139 0.000293823867121\n",
      "\n",
      "sqP = \n",
      "[[1.4379106647032125, 0.0, 0.0], [-0.04776717921869288, 0.34054287068666594, 0.0], [0.024346511897570762, 7.995068756400052e-05, 0.2459569550903684]]\n",
      "sigma = \n",
      "[[ 0.4093847   9.75520487  5.90661123]\n",
      " [ 3.28520603  9.75520487  5.90661123]\n",
      " [ 0.31385034 10.43629061  5.90661123]\n",
      " [ 0.45807772  9.75536477  6.39852514]\n",
      " [-2.46643663  9.75520487  5.90661123]\n",
      " [ 0.50491906  9.07411913  5.90661123]\n",
      " [ 0.36069167  9.75504497  5.41469732]]\n",
      "sigmax = \n",
      "[[ 0.41003335  9.84698913  5.94641902]\n",
      " [-2.99733063  9.65618749  5.89230855]\n",
      " [ 0.31449899 10.53145357  5.93748228]\n",
      " [ 0.45872638  9.84510264  6.44275323]\n",
      " [-2.46578798  9.6770988   5.84409362]\n",
      " [ 0.50556771  9.16168763  5.95499271]\n",
      " [ 0.36134033  9.84865803  5.44999044]]\n",
      "sqP = \n",
      "[[1.4391260821922063, 0.0, 0.0], [0.06284690015584303, 0.5651823390292959, 0.0], [0.03521524199474391, -0.002399359965276374, 0.5115291891531641]]\n",
      "m = \n",
      "[-0.36591135  9.87423906  5.94692092]\n",
      "P = \n",
      "[[2.06763876 0.06790297 0.03714859]\n",
      " [0.06790297 0.11879893 0.00384656]\n",
      " [0.03714859 0.00384656 0.06305101]]\n",
      "num = \n",
      "912\n",
      "SENSOR 4 3.90151568357 -0.230804046777\n",
      "\n",
      "num = \n",
      "913\n",
      "SENSOR 5 0.712985585874 0.132988889564\n",
      "\n",
      "num = \n",
      "914\n",
      "SENSOR 6 2.15622970424 -2.85121206477\n",
      "\n",
      "num = \n",
      "915\n",
      "SENSOR 7 4.91179352895 -1.43675629418\n",
      "\n",
      "num = \n",
      "916\n",
      "ODOMETRY -0.000245429391129 0.0999070378738 4.52120481994e-05\n",
      "\n",
      "sqP = \n",
      "[[1.437928635235578, 0.0, 0.0], [0.04722276771680167, 0.34142194055047387, 0.0], [0.025834790502956565, 0.007693037927175729, 0.24964852530228573]]\n",
      "sigma = \n",
      "[[-0.36591135  9.87423906  5.94692092]\n",
      " [ 2.50994592  9.87423906  5.94692092]\n",
      " [-0.27146581 10.55708294  5.94692092]\n",
      " [-0.31424177  9.88962514  6.44621797]\n",
      " [-3.24176862  9.87423906  5.94692092]\n",
      " [-0.46035688  9.19139518  5.94692092]\n",
      " [-0.41758093  9.85885299  5.44762387]]\n",
      "sigmax = \n",
      "[[-0.36483541  9.9676505   5.91121251]\n",
      " [ 2.51102186  9.79348411  6.00590756]\n",
      " [-0.27038987 10.65344556  5.92018084]\n",
      " [-0.31316583  9.98475613  6.4153816 ]\n",
      " [ 3.04249263  9.7747287   5.95684434]\n",
      " [-0.45928095  9.28102283  5.90256247]\n",
      " [-0.41650499  9.95029554  5.40713874]]\n",
      "sqP = \n",
      "[[1.439132403518079, 0.0, 0.0], [-0.06407207762832401, 0.5657001720097689, 0.0], [0.03222724481868122, 0.006001858347965339, 0.5133722559751417]]\n",
      "m = \n",
      "[0.42693971 9.81322444 5.83674326]\n",
      "P = \n",
      "[[ 2.06766101e+00 -6.89656071e-02  3.37649693e-02]\n",
      " [-6.89656071e-02  1.17442840e-01 -1.26239030e-03]\n",
      " [ 3.37649693e-02 -1.26239030e-03  6.32525463e-02]]\n",
      "num = \n",
      "917\n",
      "SENSOR 4 3.64288246527 -0.35019983237\n",
      "\n",
      "num = \n",
      "918\n",
      "SENSOR 5 0.799736995544 0.23802034997\n",
      "\n",
      "num = \n",
      "919\n",
      "SENSOR 6 2.48255730306 -2.88618660948\n",
      "\n",
      "num = \n",
      "920\n",
      "SENSOR 7 4.91606982523 -1.51036160945\n",
      "\n",
      "num = \n",
      "921\n",
      "ODOMETRY -0.000170601987707 0.0999805604304 -7.40939785008e-05\n",
      "\n",
      "sqP = \n",
      "[[1.437936371676019, 0.0, 0.0], [-0.047961515201214194, 0.3393265881124381, 0.0], [0.02348154617739743, -0.000401323593985442, 0.2504016818125093]]\n",
      "sigma = \n",
      "[[ 0.42693971  9.81322444  5.83674326]\n",
      " [ 3.30281245  9.81322444  5.83674326]\n",
      " [ 0.33101668 10.49187762  5.83674326]\n",
      " [ 0.4739028   9.8124218   6.33754663]\n",
      " [-2.44893303  9.81322444  5.83674326]\n",
      " [ 0.52286274  9.13457127  5.83674326]\n",
      " [ 0.37997662  9.81402709  5.3359399 ]]\n",
      "sigmax = \n",
      "[[ 0.42673949  9.9041737   5.87809117]\n",
      " [-2.98057307  9.71460905  5.82073016]\n",
      " [ 0.33081646 10.58636891  5.86919033]\n",
      " [ 0.47370259  9.90132967  6.38311863]\n",
      " [-2.44913325  9.73632529  5.77296289]\n",
      " [ 0.52266252  9.22114229  5.88661184]\n",
      " [ 0.3797764   9.90681718  5.37297252]]\n",
      "sqP = \n",
      "[[1.4391157989646706, 0.0, 0.0], [0.06218023544315085, 0.5644746787847432, 0.0], [0.03638555077265211, -0.0025567352830074834, 0.5136546219193856]]\n",
      "m = \n",
      "[-0.37609541  9.82737139  5.66283142]\n",
      "P = \n",
      "[[2.06758826 0.06710546 0.03834276]\n",
      " [0.06710546 0.11776999 0.00324144]\n",
      " [0.03834276 0.00324144 0.06503015]]\n",
      "num = \n",
      "922\n",
      "SENSOR 4 3.60050259878 -0.29206119154\n",
      "\n",
      "num = \n",
      "923\n",
      "SENSOR 5 0.456888182182 0.189816312354\n",
      "\n",
      "num = \n",
      "924\n",
      "SENSOR 6 2.6445990428 -2.85742387613\n",
      "\n",
      "num = \n",
      "925\n",
      "SENSOR 7 4.75304634682 -1.42550073299\n",
      "\n",
      "num = \n",
      "926\n",
      "ODOMETRY -0.00152630081795 0.0999133992614 -0.000430283518585\n",
      "\n",
      "sqP = \n",
      "[[1.4379110741577052, 0.0, 0.0], [0.04666871154053632, 0.33998827328833464, 0.0], [0.026665602345618936, 0.005873690544519299, 0.2535440635173405]]\n",
      "sigma = \n",
      "[[-0.37609541  9.82737139  5.66283142]\n",
      " [ 2.49972674  9.82737139  5.66283142]\n",
      " [-0.28275799 10.50734793  5.66283142]\n",
      " [-0.32276421  9.83911877  6.16991954]\n",
      " [-3.25191756  9.82737139  5.66283142]\n",
      " [-0.46943283  9.14739484  5.66283142]\n",
      " [-0.42942662  9.81562401  5.15574329]]\n",
      "sigmax = \n",
      "[[-0.37634011  9.92035763  5.62609353]\n",
      " [ 2.49948204  9.74729917  5.72270254]\n",
      " [-0.28300268 10.60335347  5.63491994]\n",
      " [-0.3230089   9.93393115  6.13819061]\n",
      " [ 3.03102305  9.72800055  5.67385635]\n",
      " [-0.46967753  9.23655229  5.61758694]\n",
      " [-0.42967131  9.90651969  5.11410092]]\n",
      "sqP = \n",
      "[[1.4391311054097933, 0.0, 0.0], [-0.06394831180631164, 0.5647892145730948, 0.0], [0.03327413626739768, 0.005229935665318751, 0.5153212353037055]]\n",
      "m = \n",
      "[0.40804389 9.78514744 5.47473806]\n",
      "P = \n",
      "[[ 2.06763579e+00 -6.88254976e-02  3.51769136e-02]\n",
      " [-6.88254976e-02  1.17317572e-01 -5.82100393e-04]\n",
      " [ 3.51769136e-02 -5.82100393e-04  7.08810692e-02]]\n",
      "num = \n",
      "927\n",
      "SENSOR 4 3.606832886 -0.248641284229\n",
      "\n",
      "num = \n",
      "928\n",
      "SENSOR 5 0.495940573578 0.31820180197\n",
      "\n",
      "num = \n",
      "929\n",
      "SENSOR 6 2.62200407047 -2.87985859301\n",
      "\n",
      "num = \n",
      "930\n",
      "SENSOR 7 4.75456323488 -1.62447424499\n",
      "\n",
      "num = \n",
      "931\n",
      "ODOMETRY -0.000217759654145 0.100001221848 -2.15306964727e-05\n",
      "\n",
      "sqP = \n",
      "[[1.4379276025941692, 0.0, 0.0], [-0.04786436915194874, 0.33915567867125807, 0.0], [0.024463619380873842, 0.0017361800279065073, 0.26510297285279444]]\n",
      "sigma = \n",
      "[[ 0.40804389  9.78514744  5.47473806]\n",
      " [ 3.28389909  9.78514744  5.47473806]\n",
      " [ 0.31231515 10.4634588   5.47473806]\n",
      " [ 0.45697113  9.7886198   6.00494401]\n",
      " [-2.46781132  9.78514744  5.47473806]\n",
      " [ 0.50377263  9.10683609  5.47473806]\n",
      " [ 0.35911665  9.78167508  4.94453212]]\n",
      "sigmax = \n",
      "[[ 0.4060873   9.87691824  5.51424513]\n",
      " [-3.0012428   9.6862225   5.46071866]\n",
      " [ 0.31035856 10.55858561  5.50529256]\n",
      " [ 0.45501454  9.87834858  6.0488921 ]\n",
      " [-2.4697679   9.70697327  5.4125168 ]\n",
      " [ 0.50181604  9.19441052  5.52283594]\n",
      " [ 0.35716006  9.87526826  4.97950361]]\n",
      "sqP = \n",
      "[[1.439129733409226, 0.0, 0.0], [0.06288360566617476, 0.564330345422351, 0.0], [0.03532036718033403, -0.0016905260469544628, 0.5210721797134605]]\n",
      "m = \n",
      "[-0.38813014  9.76990256  5.46642233]\n",
      "P = \n",
      "[[2.06765441 0.06775339 0.03754177]\n",
      " [0.06775339 0.11708746 0.00326822]\n",
      " [0.03754177 0.00326822 0.07448786]]\n",
      "num = \n",
      "932\n",
      "SENSOR 4 3.59697409395 -0.279183028245\n",
      "\n",
      "num = \n",
      "933\n",
      "SENSOR 5 0.480705021357 0.34438043649\n",
      "\n",
      "num = \n",
      "934\n",
      "SENSOR 6 2.63552269525 -2.87953212119\n",
      "\n",
      "num = \n",
      "935\n",
      "SENSOR 7 4.92559449029 -1.53430624815\n",
      "\n",
      "num = \n",
      "936\n",
      "ODOMETRY -0.000482506428273 0.0999122174435 -0.000580035297792\n",
      "\n",
      "sqP = \n",
      "[[1.4379340769841666, 0.0, 0.0], [0.04711856361553881, 0.33892079225489546, 0.0], [0.026108130286776214, 0.006013331730204892, 0.27160646079699885]]\n",
      "sigma = \n",
      "[[-0.38813014  9.76990256  5.46642233]\n",
      " [ 2.48773801  9.76990256  5.46642233]\n",
      " [-0.29389301 10.44774414  5.46642233]\n",
      " [-0.33591388  9.78192922  6.00963526]\n",
      " [-3.2639983   9.76990256  5.46642233]\n",
      " [-0.48236727  9.09206097  5.46642233]\n",
      " [-0.4403464   9.7578759   4.92320941]]\n",
      "sigmax = \n",
      "[[-0.38836943  9.86245728  5.42855589]\n",
      " [ 2.48749872  9.69054032  5.52726541]\n",
      " [-0.29413231 10.54345134  5.43743309]\n",
      " [-0.33615317  9.87633414  5.97665108]\n",
      " [ 3.01894772  9.67065223  5.47865412]\n",
      " [-0.48260656  9.18064188  5.42001471]\n",
      " [-0.44058569  9.84832812  4.88056391]]\n",
      "sqP = \n",
      "[[1.4391334184617095, 0.0, 0.0], [-0.06354098965795466, 0.5641839047453442, 0.0], [0.034415923969091156, 0.005525359869011819, 0.524476464546932]]\n",
      "m = \n",
      "[0.39226382 9.86398594 5.45533791]\n",
      "P = \n",
      "[[ 2.06762725e+00 -6.84240494e-02  3.62797891e-02]\n",
      " [-6.84240494e-02  1.17088535e-01 -5.14855826e-04]\n",
      " [ 3.62797891e-02 -5.14855826e-04  7.55288804e-02]]\n",
      "num = \n",
      "937\n",
      "SENSOR 4 3.47586551117 -0.349238295616\n",
      "\n",
      "num = \n",
      "938\n",
      "SENSOR 5 0.220999951716 0.388622010704\n",
      "\n",
      "num = \n",
      "939\n",
      "SENSOR 6 2.82004661589 -2.92802181092\n",
      "\n",
      "num = \n",
      "940\n",
      "SENSOR 7 4.82070833488 -1.58213543982\n",
      "\n",
      "num = \n",
      "941\n",
      "ODOMETRY -0.000139550189593 0.100064120469 -2.76254777159e-05\n",
      "\n",
      "sqP = \n",
      "[[1.4379246338669323, 0.0, 0.0], [-0.04758528212514638, 0.33885716147000505, 0.0], [0.025230661091160407, 0.0020237208435923476, 0.2736570823087828]]\n",
      "sigma = \n",
      "[[ 0.39226382  9.86398594  5.45533791]\n",
      " [ 3.26811309  9.86398594  5.45533791]\n",
      " [ 0.29709326 10.54170026  5.45533791]\n",
      " [ 0.44272514  9.86803338  6.00265207]\n",
      " [-2.48358545  9.86398594  5.45533791]\n",
      " [ 0.48743439  9.18627161  5.45533791]\n",
      " [ 0.3418025   9.8599385   4.90802375]]\n",
      "sigmax = \n",
      "[[ 0.39120128  9.95632784  5.49348793]\n",
      " [-3.01613476  9.76486625  5.44277849]\n",
      " [ 0.29603072 10.63724957  5.48454032]\n",
      " [ 0.4416626   9.95833346  6.04541125]\n",
      " [-2.48464799  9.78490467  5.39427561]\n",
      " [ 0.48637184  9.27457036  5.50209026]\n",
      " [ 0.34073996  9.95408714  4.94146748]]\n",
      "sqP = \n",
      "[[1.4391339956097988, 0.0, 0.0], [0.06340952687177588, 0.5641116957463522, 0.0], [0.03451595939334769, -0.001605871626879889, 0.5255398293433896]]\n",
      "m = \n",
      "[-0.41039986  9.85832437  5.31867079]\n",
      "P = \n",
      "[[2.06764877 0.06821377 0.03652659]\n",
      " [0.06821377 0.11650021 0.00258017]\n",
      " [0.03652659 0.00258017 0.07604997]]\n",
      "num = \n",
      "942\n",
      "SENSOR 4 3.36308480601 -0.338986772029\n",
      "\n",
      "num = \n",
      "943\n",
      "SENSOR 5 0.444200909715 0.716185023584\n",
      "\n",
      "num = \n",
      "944\n",
      "SENSOR 6 2.87114105271 -2.87495975842\n",
      "\n",
      "num = \n",
      "945\n",
      "SENSOR 7 5.00458709072 -1.58257275001\n",
      "\n",
      "num = \n",
      "946\n",
      "ODOMETRY 0.000625023111235 0.100033704567 0.000788974348719\n",
      "\n",
      "sqP = \n",
      "[[1.4379321148790647, 0.0, 0.0], [0.047438795635549195, 0.3380085323842018, 0.0], [0.025402168993207214, 0.004068296922103386, 0.27456903024248563]]\n",
      "sigma = \n",
      "[[-0.41039986  9.85832437  5.31867079]\n",
      " [ 2.46546437  9.85832437  5.31867079]\n",
      " [-0.31552226 10.53434143  5.31867079]\n",
      " [-0.35959552  9.86646096  5.86780885]\n",
      " [-3.28626409  9.85832437  5.31867079]\n",
      " [-0.50527745  9.1823073   5.31867079]\n",
      " [-0.46120419  9.85018777  4.76953273]]\n",
      "sigmax = \n",
      "[[-0.41056703  9.95007373  5.2787348 ]\n",
      " [ 2.4652972   9.78028295  5.38129959]\n",
      " [-0.31568944 10.6294615   5.28760632]\n",
      " [-0.35976269  9.96011999  5.83258365]\n",
      " [ 2.99675405  9.7593076   5.33311058]\n",
      " [-0.50544462  9.26986067  5.27022251]\n",
      " [-0.46137137  9.9397907   4.72498901]]\n",
      "sqP = \n",
      "[[1.4391314858390043, 0.0, 0.0], [-0.06290715512531439, 0.5636577330473054, 0.0], [0.03600558531482914, 0.004551322583748106, 0.5259962551253318]]\n",
      "m = \n",
      "[0.36853965 9.93554403 5.25709153]\n",
      "P = \n",
      "[[ 2.06760786e+00 -6.77236080e-02  3.81881019e-02]\n",
      " [-6.77236080e-02  1.16172480e-01 -1.51823247e-03]\n",
      " [ 3.81881019e-02 -1.51823247e-03  7.87373522e-02]]\n",
      "num = \n",
      "947\n",
      "SENSOR 4 3.13679295783 -0.355723706289\n",
      "\n",
      "num = \n",
      "948\n",
      "SENSOR 5 0.132273645824 1.00432805138\n",
      "\n",
      "num = \n",
      "949\n",
      "SENSOR 6 3.04078654628 -2.86651045039\n",
      "\n",
      "num = \n",
      "950\n",
      "SENSOR 7 4.91504091129 -1.62691182927\n",
      "\n",
      "num = \n",
      "951\n",
      "ODOMETRY -0.000430603052429 0.100031372434 -0.000194224656002\n",
      "\n",
      "sqP = \n",
      "[[1.4379178910802994, 0.0, 0.0], [-0.0470983833326952, 0.33757106320059355, 0.0], [0.026557915539259555, -0.0007921223455766616, 0.2793410135505558]]\n",
      "sigma = \n",
      "[[ 0.36853965  9.93554403  5.25709153]\n",
      " [ 3.24437543  9.93554403  5.25709153]\n",
      " [ 0.27434288 10.61068615  5.25709153]\n",
      " [ 0.42165548  9.93395978  5.81577356]\n",
      " [-2.50729613  9.93554403  5.25709153]\n",
      " [ 0.46273642  9.2604019   5.25709153]\n",
      " [ 0.31542382  9.93712827  4.69840951]]\n",
      "sigmax = \n",
      "[[ 0.36995365 10.02883837  5.29318735]\n",
      " [-3.03739588  9.83604469  5.24676569]\n",
      " [ 0.27575688 10.70696198  5.28425229]\n",
      " [ 0.42306948 10.02520619  5.85677154]\n",
      " [-2.50588213  9.85500507  5.19776011]\n",
      " [ 0.46415041  9.34988757  5.30180236]\n",
      " [ 0.31683782 10.0322074   4.72950134]]\n",
      "sqP = \n",
      "[[1.4391424823388952, 0.0, 0.0], [0.06425902043169167, 0.5632830125569549, 0.0], [0.03321402657956583, -0.00309124250463313, 0.5286010437638863]]\n",
      "m = \n",
      "[-0.43528883  9.91464143  5.08547861]\n",
      "P = \n",
      "[[2.06766271e+00 6.90732649e-02 3.52268223e-02]\n",
      " [6.90732649e-02 1.15445477e-01 1.84965293e-03]\n",
      " [3.52268223e-02 1.84965293e-03 7.99196735e-02]]\n",
      "num = \n",
      "952\n",
      "SENSOR 4 2.89257924978 -0.261154689677\n",
      "\n",
      "num = \n",
      "953\n",
      "SENSOR 5 0.0869154821736 1.57743143981\n",
      "\n",
      "num = \n",
      "954\n",
      "SENSOR 6 2.90779553154 -2.87205172826\n",
      "\n",
      "num = \n",
      "955\n",
      "SENSOR 7 4.87504647749 -1.67682352072\n",
      "\n",
      "num = \n",
      "956\n",
      "ODOMETRY -0.000394366473818 0.0999652691747 0.000203739446394\n",
      "\n",
      "sqP = \n",
      "[[1.437936962147395, 0.0, 0.0], [0.04803636507358423, 0.3363599037183435, 0.0], [0.024498168727929315, 0.0020003869225776226, 0.28163009721379834]]\n",
      "sigma = \n",
      "[[-0.43528883  9.91464143  5.08547861]\n",
      " [ 2.44058509  9.91464143  5.08547861]\n",
      " [-0.3392161  10.58736123  5.08547861]\n",
      " [-0.38629249  9.9186422   5.64873881]\n",
      " [-3.31116275  9.91464143  5.08547861]\n",
      " [-0.53136156  9.24192162  5.08547861]\n",
      " [-0.48428517  9.91064065  4.52221842]]\n",
      "sigmax = \n",
      "[[-0.43591366 10.00532653  5.0432591 ]\n",
      " [ 2.43996027  9.83822597  5.15003056]\n",
      " [-0.33984093 10.68167806  5.05215276]\n",
      " [-0.38691732 10.01128625  5.61101142]\n",
      " [ 2.97139773  9.81605204  5.10240223]\n",
      " [-0.53198639  9.32813863  5.03475482]\n",
      " [-0.48490999  9.99914915  4.47560811]]\n",
      "sqP = \n",
      "[[1.4391351877496767, 0.0, 0.0], [-0.06202685338313856, 0.5627063913115395, 0.0], [0.037799432577322246, 0.003514270275117731, 0.5296967644543077]]\n",
      "m = \n",
      "[0.36048899 9.82338841 5.00683324]\n",
      "P = \n",
      "[[ 2.06759874e+00 -6.67580488e-02  4.01053670e-02]\n",
      " [-6.67580488e-02  1.15210471e-01 -2.03749119e-03]\n",
      " [ 4.01053670e-02 -2.03749119e-03  8.12029426e-02]]\n",
      "num = \n",
      "957\n",
      "SENSOR 4 2.95060694926 -0.40938842696\n",
      "\n",
      "num = \n",
      "958\n",
      "SENSOR 5 0.187834365445 2.26857479846\n",
      "\n",
      "num = \n",
      "959\n",
      "SENSOR 6 3.31126955451 -2.92453711135\n",
      "\n",
      "num = \n",
      "960\n",
      "SENSOR 7 5.00762306838 -1.62193179913\n",
      "\n",
      "num = \n",
      "961\n",
      "ODOMETRY 0.000188872633703 0.0998900920204 -0.000404695062714\n",
      "\n",
      "sqP = \n",
      "[[1.4379147184805685, 0.0, 0.0], [-0.0464269875688843, 0.33623653267294623, 0.0], [0.027891339118154137, -0.0022085057964075093, 0.2835844464284182]]\n",
      "sigma = \n",
      "[[ 0.36048899  9.82338841  5.00683324]\n",
      " [ 3.23631843  9.82338841  5.00683324]\n",
      " [ 0.26763501 10.49586147  5.00683324]\n",
      " [ 0.41627167  9.81897139  5.57400213]\n",
      " [-2.51534045  9.82338841  5.00683324]\n",
      " [ 0.45334296  9.15091534  5.00683324]\n",
      " [ 0.30470631  9.82780542  4.43966435]]\n",
      "sigmax = \n",
      "[[ 0.36029836  9.91694225  5.04205728]\n",
      " [-3.04705751  9.72386757  4.99741735]\n",
      " [ 0.26744439 10.5922783   5.03323117]\n",
      " [ 0.41608104  9.91041585  5.61438736]\n",
      " [-2.51553108  9.74237046  4.94827431]\n",
      " [ 0.45315234  9.24080018  5.05057991]\n",
      " [ 0.30451568  9.92317762  4.46961762]]\n",
      "sqP = \n",
      "[[1.4391461840289133, 0.0, 0.0], [0.06463036366280067, 0.5624353682010681, 0.0], [0.032852435512482465, -0.0038721117894259124, 0.530921811165941]]\n",
      "m = \n",
      "[-0.42614466  9.94657585  4.86929288]\n",
      "P = \n",
      "[[2.06767392e+00 6.96650797e-02 3.47639475e-02]\n",
      " [6.96650797e-02 1.16303274e-01 1.83233205e-03]\n",
      " [3.47639475e-02 1.83233205e-03 7.99570938e-02]]\n",
      "num = \n",
      "962\n",
      "SENSOR 4 2.71804063576 -0.263896340835\n",
      "\n",
      "num = \n",
      "963\n",
      "SENSOR 5 0.117592609818 2.55977634237\n",
      "\n",
      "num = \n",
      "964\n",
      "SENSOR 6 3.49353272518 -2.94938522616\n",
      "\n",
      "num = \n",
      "965\n",
      "SENSOR 7 4.88923424474 -1.71729987801\n",
      "\n",
      "num = \n",
      "966\n",
      "ODOMETRY 0.000463123137535 0.100026190262 -0.000467923097136\n",
      "\n",
      "sqP = \n",
      "[[1.4379408608420428, 0.0, 0.0], [0.04844780590201108, 0.3375738197871669, 0.0], [0.024176201152950865, 0.0019582328646887558, 0.28172463586707747]]\n",
      "sigma = \n",
      "[[-0.42614466  9.94657585  4.86929288]\n",
      " [ 2.44973706  9.94657585  4.86929288]\n",
      " [-0.32924905 10.62172348  4.86929288]\n",
      " [-0.37779226  9.95049231  5.43274215]\n",
      " [-3.30202638  9.94657585  4.86929288]\n",
      " [-0.52304027  9.27142821  4.86929288]\n",
      " [-0.47449706  9.94265938  4.30584361]]\n",
      "sigmax = \n",
      "[[-0.42636048 10.03754018  4.82801916]\n",
      " [ 2.44952124  9.86964209  4.93300495]\n",
      " [-0.32946487 10.71625412  4.83701302]\n",
      " [-0.37800808 10.04334524  5.3959133 ]\n",
      " [ 2.98094311  9.84796552  4.88523134]\n",
      " [-0.52325609  9.35797287  4.8194125 ]\n",
      " [-0.47471288 10.03152249  4.26022149]]\n",
      "sqP = \n",
      "[[1.439143175169417, 0.0, 0.0], [-0.06210419554669825, 0.5634423633314811, 0.0], [0.036976424688117036, 0.0035176054471388633, 0.5297408511660154]]\n",
      "m = \n",
      "[0.35730704 9.89975844 4.67854086]\n",
      "P = \n",
      "[[ 2.06765317e+00 -6.67491408e-02  3.92375350e-02]\n",
      " [-6.67491408e-02  1.14980393e-01 -1.96056509e-03]\n",
      " [ 3.92375350e-02 -1.96056509e-03  8.12719830e-02]]\n",
      "num = \n",
      "967\n",
      "SENSOR 4 2.79334888537 -0.399856478188\n",
      "\n",
      "num = \n",
      "968\n",
      "SENSOR 5 0.493078840954 2.63845120424\n",
      "\n",
      "num = \n",
      "969\n",
      "SENSOR 6 3.4746419933 -2.96022159027\n",
      "\n",
      "num = \n",
      "970\n",
      "SENSOR 7 4.852250925 -1.62247582516\n",
      "\n",
      "num = \n",
      "971\n",
      "ODOMETRY 0.000204722786588 0.100025452409 -0.000621143247794\n",
      "\n",
      "sqP = \n",
      "[[1.437933644207805, 0.0, 0.0], [-0.046420181541360725, 0.3358951617103462, 0.0], [0.027287444816809822, -0.0020657545212702513, 0.2837659440214415]]\n",
      "sigma = \n",
      "[[ 0.35730704  9.89975844  4.67854086]\n",
      " [ 3.23317433  9.89975844  4.67854086]\n",
      " [ 0.26446668 10.57154876  4.67854086]\n",
      " [ 0.41188193  9.89562693  5.24607274]\n",
      " [-2.51856024  9.89975844  4.67854086]\n",
      " [ 0.45014741  9.22796811  4.67854086]\n",
      " [ 0.30273216  9.90388994  4.11100897]]\n",
      "sigmax = \n",
      "[[ 0.35730224  9.99345097  4.71356867]\n",
      " [-3.05001577  9.80015567  4.66934696]\n",
      " [ 0.26446188 10.66808512  4.70473187]\n",
      " [ 0.41187713  9.98726928  5.28615913]\n",
      " [-2.51856504  9.81855293  4.62013791]\n",
      " [ 0.45014261  9.31800983  4.72210378]\n",
      " [ 0.30272736  9.99935367  4.14087391]]\n",
      "sqP = \n",
      "[[1.439132640004894, 0.0, 0.0], [0.06475860956131713, 0.5622257233725572, 0.0], [0.032576711926646296, -0.003781320304449489, 0.531000010036328]]\n",
      "m = \n",
      "[-0.43933     9.86302817  4.67172609]\n",
      "P = \n",
      "[[2.06762661e+00 6.97466585e-02 3.44136742e-02]\n",
      " [6.97466585e-02 1.15805382e-01 1.65105866e-03]\n",
      " [3.44136742e-02 1.65105866e-03 7.93917710e-02]]\n",
      "num = \n",
      "972\n",
      "SENSOR 4 2.73084876629 -0.369067978649\n",
      "\n",
      "num = \n",
      "973\n",
      "SENSOR 5 0.42617115042 2.86523242658\n",
      "\n",
      "num = \n",
      "974\n",
      "SENSOR 6 3.81253344685 -2.96532036628\n",
      "\n",
      "num = \n",
      "975\n",
      "SENSOR 7 4.96857297672 -1.68391105412\n",
      "\n",
      "num = \n",
      "976\n",
      "ODOMETRY -0.000217736658741 0.0999861864311 -0.000591307921892\n",
      "\n",
      "sqP = \n",
      "[[1.4379244086764802, 0.0, 0.0], [0.04850509389725897, 0.33682731186167164, 0.0], [0.02393288133334914, 0.0014553214122455685, 0.28074342421869564]]\n",
      "sigma = \n",
      "[[-0.43933     9.86302817  4.67172609]\n",
      " [ 2.43651882  9.86302817  4.67172609]\n",
      " [-0.34231981 10.5366828   4.67172609]\n",
      " [-0.39146424  9.86593882  5.23321294]\n",
      " [-3.31517882  9.86302817  4.67172609]\n",
      " [-0.53634019  9.18937355  4.67172609]\n",
      " [-0.48719576  9.86011753  4.11023924]]\n",
      "sigmax = \n",
      "[[-0.43974642  9.9535636   4.62920048]\n",
      " [ 2.4361024   9.78683915  4.736536  ]\n",
      " [-0.34273623 10.6309115   4.63816952]\n",
      " [-0.39188066  9.95840529  5.19506793]\n",
      " [ 2.96759007  9.7645024   4.68898189]\n",
      " [-0.53675661  9.27536435  4.62063134]\n",
      " [-0.48761218  9.94851452  4.06343045]]\n",
      "sqP = \n",
      "[[1.4391425567926273, 0.0, 0.0], [-0.06179280131092733, 0.5630142615163969, 0.0], [0.03792190820850961, 0.0032211108921954687, 0.5292035187679358]]\n",
      "m = \n",
      "[0.33073026 9.95859008 4.51916028]\n",
      "P = \n",
      "[[ 2.06763324 -0.06658783  0.04011181]\n",
      " [-0.06658783  0.11597401 -0.00246448]\n",
      " [ 0.04011181 -0.00246448  0.07859761]]\n",
      "num = \n",
      "977\n",
      "SENSOR 4 2.6677545575 -0.3747009033\n",
      "\n",
      "num = \n",
      "978\n",
      "SENSOR 5 0.7159982817 2.86732646754\n",
      "\n",
      "num = \n",
      "979\n",
      "SENSOR 6 3.60775134837 -3.03170946031\n",
      "\n",
      "num = \n",
      "980\n",
      "SENSOR 7 4.85926199224 -1.74229396012\n",
      "\n",
      "num = \n",
      "981\n",
      "ODOMETRY -0.000184559437448 0.100062080709 -0.000284379235048\n",
      "\n",
      "sqP = \n",
      "[[1.4379267160334377, 0.0, 0.0], [-0.046308223436221624, 0.337386365113541, 0.0], [0.02789558439308904, -0.0034757824934887856, 0.27893972067143136]]\n",
      "sigma = \n",
      "[[ 0.33073026  9.95859008  4.51916028]\n",
      " [ 3.20658369  9.95859008  4.51916028]\n",
      " [ 0.23811382 10.63336281  4.51916028]\n",
      " [ 0.38652143  9.95163851  5.07703972]\n",
      " [-2.54512317  9.95859008  4.51916028]\n",
      " [ 0.42334671  9.28381735  4.51916028]\n",
      " [ 0.27493909  9.96554164  3.96128084]]\n",
      "sigmax = \n",
      "[[ 0.32992122 10.05316463  4.55160858]\n",
      " [-3.07741066  9.85881357  4.51268837]\n",
      " [ 0.23730477 10.73053298  4.54272287]\n",
      " [ 0.38571239 10.04425652  5.11471123]\n",
      " [-2.54593221  9.87585689  4.46301354]\n",
      " [ 0.42253766  9.37498561  4.56021616]\n",
      " [ 0.27413005 10.06177843  3.98840497]]\n",
      "sqP = \n",
      "[[1.4391413321333686, 0.0, 0.0], [0.06538952175043322, 0.5630878215844373, 0.0], [0.030522587427577953, -0.004523553683576891, 0.5284574794592894]]\n",
      "m = \n",
      "[-0.47165815  9.87322638  4.52376595]\n",
      "P = \n",
      "[[2.06768321e+00 7.03290543e-02 3.22914931e-02]\n",
      " [7.03290543e-02 1.15380097e-01 1.86835354e-03]\n",
      " [3.22914931e-02 1.86835354e-03 7.74037281e-02]]\n",
      "num = \n",
      "982\n",
      "SENSOR 4 2.47524626759 -0.497455287779\n",
      "\n",
      "num = \n",
      "983\n",
      "SENSOR 5 0.515976055959 2.86747266043\n",
      "\n",
      "num = \n",
      "984\n",
      "SENSOR 6 3.71140327246 -2.89739069321\n",
      "\n",
      "num = \n",
      "985\n",
      "SENSOR 7 4.86833979771 -1.86747905373\n",
      "\n",
      "num = \n",
      "986\n",
      "ODOMETRY -0.000423831010905 0.100055729458 0.000522719071377\n",
      "\n",
      "sqP = \n",
      "[[1.4379440918551405, 0.0, 0.0], [0.04890944973122879, 0.33613682164593284, 0.0], [0.02245670971181059, 0.002290758319710215, 0.27729799253739806]]\n",
      "sigma = \n",
      "[[-0.47165815  9.87322638  4.52376595]\n",
      " [ 2.40423003  9.87322638  4.52376595]\n",
      " [-0.37383925 10.54550002  4.52376595]\n",
      " [-0.42674473  9.8778079   5.07836194]\n",
      " [-3.34754634  9.87322638  4.52376595]\n",
      " [-0.56947705  9.20095274  4.52376595]\n",
      " [-0.51657157  9.86864486  3.96916997]]\n",
      "sigmax = \n",
      "[[-0.47212709  9.9623549   4.4782849 ]\n",
      " [ 2.40376109  9.79916841  4.59105516]\n",
      " [-0.37430819 10.63864429  4.48720687]\n",
      " [-0.42721367  9.96888856  5.03692847]\n",
      " [ 2.93517003  9.77528275  4.54424681]\n",
      " [-0.56994599  9.28521337  4.46979776]\n",
      " [-0.51704051  9.95564148  3.91973305]]\n",
      "sqP = \n",
      "[[1.4391306332702514, 0.0, 0.0], [-0.0606118931008875, 0.5626705878198804, 0.0], [0.039968314959661716, 0.0034856808356516543, 0.5273031499925763]]\n",
      "m = \n",
      "[0.31654779 9.84188924 4.43573508]\n",
      "P = \n",
      "[[ 2.06757023 -0.06537797  0.0421885 ]\n",
      " [-0.06537797  0.11589526 -0.00262263]\n",
      " [ 0.0421885  -0.00262263  0.07623361]]\n",
      "num = \n",
      "987\n",
      "SENSOR 4 2.40614223049 -0.363441019406\n",
      "\n",
      "num = \n",
      "988\n",
      "SENSOR 5 0.586364241415 2.87684153835\n",
      "\n",
      "num = \n",
      "989\n",
      "SENSOR 6 3.90284744099 -2.93101327542\n",
      "\n",
      "num = \n",
      "990\n",
      "SENSOR 7 4.77611939003 -1.8279190371\n",
      "\n",
      "num = \n",
      "991\n",
      "ODOMETRY -0.000503367053185 0.100103555799 0.000819633507756\n",
      "\n",
      "sqP = \n",
      "[[1.4379048049979986, 0.0, 0.0], [-0.04546752072199526, 0.3373840006004061, 0.0], [0.029340259054327727, -0.00381940000778488, 0.2745144372698595]]\n",
      "sigma = \n",
      "[[ 0.31654779  9.84188924  4.43573508]\n",
      " [ 3.1923574   9.84188924  4.43573508]\n",
      " [ 0.22561275 10.51665725  4.43573508]\n",
      " [ 0.37522831  9.83425044  4.98476395]\n",
      " [-2.55926182  9.84188924  4.43573508]\n",
      " [ 0.40748283  9.16712124  4.43573508]\n",
      " [ 0.25786727  9.84952804  3.8867062 ]]\n",
      "sigmax = \n",
      "[[ 0.31664668  9.93698697  4.4668409 ]\n",
      " [-3.09072902  9.74196027  4.43070031]\n",
      " [ 0.22571164 10.61418676  4.45807657]\n",
      " [ 0.3753272   9.92736023  5.02139341]\n",
      " [-2.55916293  9.75830108  4.38074264]\n",
      " [ 0.40758172  9.25900134  4.47534818]\n",
      " [ 0.25796616  9.94628634  3.9121813 ]]\n",
      "sqP = \n",
      "[[1.439145276895884, 0.0, 0.0], [0.06598221089132271, 0.5630293372539956, 0.0], [0.02962820645250887, -0.004695677006720888, 0.5261850938628412]]\n",
      "m = \n",
      "[-0.4868444   9.79359662  4.33330814]\n",
      "P = \n",
      "[[2.06768211e+00 7.10913878e-02 3.11306428e-02]\n",
      " [7.10913878e-02 1.16987650e-01 7.68450716e-04]\n",
      " [3.11306428e-02 7.68450716e-04 7.41307836e-02]]\n",
      "num = \n",
      "992\n",
      "SENSOR 4 2.40160042935 -0.389154949411\n",
      "\n",
      "num = \n",
      "993\n",
      "SENSOR 5 0.853827928964 3.00466868631\n",
      "\n",
      "num = \n",
      "994\n",
      "SENSOR 6 3.82496452269 -2.95364332599\n",
      "\n",
      "num = \n",
      "995\n",
      "SENSOR 7 5.02564796909 -1.78367151428\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "num = \n",
      "996\n",
      "ODOMETRY -0.00080944386785 0.100013043879 1.97022648298e-05\n",
      "\n",
      "sqP = \n",
      "[[1.4379437078900463, 0.0, 0.0], [0.0494396181063128, 0.3384425716237032, 0.0], [0.021649416926604898, -0.0008919923638493523, 0.27140613615149495]]\n",
      "sigma = \n",
      "[[-0.4868444   9.79359662  4.33330814]\n",
      " [ 2.38904301  9.79359662  4.33330814]\n",
      " [-0.38796517 10.47048176  4.33330814]\n",
      " [-0.44354557  9.79181263  4.87612041]\n",
      " [-3.36273182  9.79359662  4.33330814]\n",
      " [-0.58572364  9.11671147  4.33330814]\n",
      " [-0.53014324  9.7953806   3.79049586]]\n",
      "sigmax = \n",
      "[[-0.48652814  9.88204591  4.28643123]\n",
      " [ 2.38935928  9.72056062  4.4017659 ]\n",
      " [-0.3876489  10.56312662  4.29539176]\n",
      " [-0.4432293   9.88220811  4.833116  ]\n",
      " [ 2.92076976  9.69594182  4.35531413]\n",
      " [-0.58540737  9.20010113  4.27792865]\n",
      " [-0.52982697  9.88171792  3.73983434]]\n",
      "sqP = \n",
      "[[1.4391364134945277, 0.0, 0.0], [-0.059925219028670235, 0.5641045787890255, 0.0], [0.04085312793049115, 0.0018528069676519146, 0.5241881149191715]]\n",
      "m = \n",
      "[0.29151618 9.88731677 4.28188491]\n",
      "P = \n",
      "[[ 2.06758241 -0.06468705  0.04293904]\n",
      " [-0.06468705  0.11717693 -0.00375594]\n",
      " [ 0.04293904 -0.00375594  0.07080342]]\n",
      "num = \n",
      "997\n",
      "SENSOR 4 2.14265941231 -0.576114496391\n",
      "\n",
      "num = \n",
      "998\n",
      "SENSOR 5 1.06875104344 3.01158775394\n",
      "\n",
      "num = \n",
      "999\n",
      "SENSOR 6 4.04413273461 -2.9178294235\n",
      "\n",
      "num = \n",
      "1000\n",
      "SENSOR 7 5.0221824592 -1.79251846775\n",
      "\n",
      "num = \n",
      "1001\n",
      "ODOMETRY -0.000993208621031 0.0998844685408 -0.000121644357505\n",
      "\n",
      "sqP = \n",
      "[[1.437909039827318, 0.0, 0.0], [-0.044986885179936355, 0.33934217694118674, 0.0], [0.029862137880391645, -0.007109451335273476, 0.264312552071973]]\n",
      "sigma = \n",
      "[[ 0.29151618  9.88731677  4.28188491]\n",
      " [ 3.16733426  9.88731677  4.28188491]\n",
      " [ 0.20154241 10.56600112  4.28188491]\n",
      " [ 0.35124045  9.87309786  4.81051001]\n",
      " [-2.5843019   9.88731677  4.28188491]\n",
      " [ 0.38148995  9.20863241  4.28188491]\n",
      " [ 0.2317919   9.90153567  3.7532598 ]]\n",
      "sigmax = \n",
      "[[ 0.29072643  9.98313342  4.31055158]\n",
      " [-3.11664079  9.78733481  4.27939162]\n",
      " [ 0.20075266 10.66400597  4.30182627]\n",
      " [ 0.35045071  9.9670326   4.84484475]\n",
      " [-2.58509165  9.8023938   4.22905785]\n",
      " [ 0.3807002   9.30148572  4.31904499]\n",
      " [ 0.23100216  9.99889256  3.77615619]]\n",
      "sqP = \n",
      "[[1.439140515501818, 0.0, 0.0], [0.06651787622562302, 0.5641938461882261, 0.0], [0.02754856353481884, -0.006185343504260112, 0.5209168402217335]]\n",
      "m = \n",
      "[-0.51239068  9.88446526  4.0817872 ]\n",
      "P = \n",
      "[[2.06770298e+00 7.16089830e-02 2.89844969e-02]\n",
      " [7.16089830e-02 1.16961024e-01 9.48684105e-04]\n",
      " [2.89844969e-02 9.48684105e-04 7.00872855e-02]]\n",
      "num = \n",
      "1002\n",
      "SENSOR 4 2.27760317493 -0.454019833416\n",
      "\n",
      "num = \n",
      "1003\n",
      "SENSOR 5 0.989955134561 3.00334167163\n",
      "\n",
      "num = \n",
      "1004\n",
      "SENSOR 6 4.0746975399 -2.97885744051\n",
      "\n",
      "num = \n",
      "1005\n",
      "ODOMETRY -0.000250538104947 0.100118558009 0.000470538017319\n",
      "\n",
      "sqP = \n",
      "[[1.4379509652677012, 0.0, 0.0], [0.04979932187805818, 0.33835048664617334, 0.0], [0.020156804818580176, -0.00016288171042083146, 0.26397151776496963]]\n",
      "sigma = \n",
      "[[-0.51239068  9.88446526  4.0817872 ]\n",
      " [ 2.36351125  9.88446526  4.0817872 ]\n",
      " [-0.41279203 10.56116623  4.0817872 ]\n",
      " [-0.47207707  9.8841395   4.60973024]\n",
      " [-3.38829261  9.88446526  4.0817872 ]\n",
      " [-0.61198932  9.20776429  4.0817872 ]\n",
      " [-0.55270429  9.88479102  3.55384416]]\n",
      "sigmax = \n",
      "[[-0.51350553  9.97147338  4.03273117]\n",
      " [ 2.3623964   9.81339106  4.15196815]\n",
      " [-0.41390689 10.65262099  4.04162585]\n",
      " [-0.47319192  9.97305401  4.56422072]\n",
      " [ 2.89377785  9.78762921  4.10627569]\n",
      " [-0.61310417  9.28946337  4.02432271]\n",
      " [-0.55381914  9.96975136  3.50132133]]\n",
      "sqP = \n",
      "[[1.4391264657852536, 0.0, 0.0], [-0.05869878744146377, 0.5640954444826405, 0.0], [0.04223211591930918, 0.002060449503304862, 0.5202977939214559]]\n",
      "m = \n",
      "[0.2752386  9.91041748 4.0747124 ]\n",
      "P = \n",
      "[[ 2.06922534 -0.07856222  0.04552422]\n",
      " [-0.07856222  0.25261872 -0.01688933]\n",
      " [ 0.04552422 -0.01688933  0.06566566]]\n",
      "num = \n",
      "1006\n",
      "SENSOR 8 5.08620163194 -1.49046025052\n",
      "\n",
      "num = \n",
      "1007\n",
      "SENSOR 4 2.30195764953 -0.428336448448\n",
      "\n",
      "num = \n",
      "1008\n",
      "SENSOR 5 1.20549687351 2.9880742534\n",
      "\n",
      "num = \n",
      "1009\n",
      "SENSOR 6 4.35196037607 -2.99771407728\n",
      "\n",
      "num = \n",
      "1010\n",
      "ODOMETRY -0.000419971039834 0.0999850157042 -0.000136873067745\n",
      "\n",
      "sqP = \n",
      "[[1.438480220236545, 0.0, 0.0], [-0.05461473834355931, 0.4996358164557407, 0.0], [0.031647442248721576, -0.03034393585684665, 0.2524744455128173]]\n",
      "sigma = \n",
      "[[ 0.2752386   9.91041748  4.0747124 ]\n",
      " [ 3.15219904  9.91041748  4.0747124 ]\n",
      " [ 0.16600913 10.90968911  4.0747124 ]\n",
      " [ 0.33853349  9.84972961  4.5796613 ]\n",
      " [-2.60172184  9.91041748  4.0747124 ]\n",
      " [ 0.38446808  8.91114585  4.0747124 ]\n",
      " [ 0.21194372  9.97110535  3.56976351]]\n",
      "sigmax = \n",
      "[[ 0.2754586  10.00677443  4.10189814]\n",
      " [-3.13076626  9.81030429  4.07367561]\n",
      " [ 0.16622913 11.00843539  4.09123202]\n",
      " [ 0.33875349  9.94417403  4.61288743]\n",
      " [-2.60150184  9.82452538  4.02327046]\n",
      " [ 0.38468808  9.00396496  4.11224023]\n",
      " [ 0.21216372 10.06898892  3.59079998]]\n",
      "sqP = \n",
      "[[1.4394591718781873, 0.0, 0.0], [0.05801196935179814, 0.6748665597213486, 0.0], [0.026491605260892018, -0.015254886715155366, 0.5148563006295119]]\n",
      "m = \n",
      "[-0.51403983  9.99271883  3.95927051]\n",
      "P = \n",
      "[[ 2.06899497  0.05526022  0.02665742]\n",
      " [ 0.05526022  0.13551518 -0.01718366]\n",
      " [ 0.02665742 -0.01718366  0.066907  ]]\n",
      "num = \n",
      "1011\n",
      "SENSOR 8 4.9606373049 -1.48374082501\n",
      "\n",
      "num = \n",
      "1012\n",
      "SENSOR 4 2.02122842241 -0.612520417005\n",
      "\n",
      "num = \n",
      "1013\n",
      "SENSOR 5 1.18548266277 2.91183826645\n",
      "\n",
      "num = \n",
      "1014\n",
      "SENSOR 6 4.40601584512 -3.00750175779\n",
      "\n",
      "num = \n",
      "1015\n",
      "ODOMETRY 0.000258908008153 0.0999880869982 -0.000878618195419\n",
      "\n",
      "sqP = \n",
      "[[1.4384001408399427, 0.0, 0.0], [0.03841783396939978, 0.366113705476453, 0.0], [0.018532683814895007, -0.04888003144811482, 0.25332643629911344]]\n",
      "sigma = \n",
      "[[-0.51403983  9.99271883  3.95927051]\n",
      " [ 2.36276045  9.99271883  3.95927051]\n",
      " [-0.43720417 10.72494624  3.95927051]\n",
      " [-0.47697447  9.89495877  4.46592338]\n",
      " [-3.39084012  9.99271883  3.95927051]\n",
      " [-0.5908755   9.26049142  3.95927051]\n",
      " [-0.5511052  10.09047889  3.45261764]]\n",
      "sigmax = \n",
      "[[-0.51459668 10.07976165  3.91007142]\n",
      " [ 2.3622036   9.92158556  4.02953474]\n",
      " [-0.43776101 10.81550878  3.91689799]\n",
      " [-0.47753131  9.98376497  4.41998362]\n",
      " [ 2.89178835  9.89583388  3.98397498]\n",
      " [-0.59143235  9.3435009   3.90353516]\n",
      " [-0.55166205 10.17563876  3.4002268 ]]\n",
      "sqP = \n",
      "[[1.4387143446827646, 0.0, 0.0], [-0.06137371205350861, 0.5826494615351602, 0.0], [0.0418854353917641, -0.019656232723833576, 0.514533442977709]]\n",
      "m = \n",
      "[0.27435529 9.91982471 3.82081627]\n",
      "P = \n",
      "[[ 2.06622109 -0.06476889  0.04476261]\n",
      " [-0.06476889  0.11860438 -0.015736  ]\n",
      " [ 0.04476261 -0.015736    0.06426413]]\n",
      "num = \n",
      "1016\n",
      "SENSOR 8 5.07928480298 -1.47517883113\n",
      "\n",
      "num = \n",
      "1017\n",
      "SENSOR 4 1.95540196161 -0.524791774647\n",
      "\n",
      "num = \n",
      "1018\n",
      "SENSOR 5 1.50601099601 3.02951420464\n",
      "\n",
      "num = \n",
      "1019\n",
      "SENSOR 6 4.2697515621 -2.91223271519\n",
      "\n",
      "num = \n",
      "1020\n",
      "ODOMETRY 8.35160305286e-05 0.0998921936279 -0.000916685935868\n",
      "\n",
      "sqP = \n",
      "[[1.43743559648251, 0.0, 0.0], [-0.04505864062693721, 0.34142949440863546, 0.0], [0.03114060255345023, -0.04197893777440714, 0.24805676990162995]]\n",
      "sigma = \n",
      "[[ 0.27435529  9.91982471  3.82081627]\n",
      " [ 3.14922649  9.91982471  3.82081627]\n",
      " [ 0.18423801 10.6026837   3.82081627]\n",
      " [ 0.3366365   9.83586683  4.31692981]\n",
      " [-2.6005159   9.91982471  3.82081627]\n",
      " [ 0.36447257  9.23696572  3.82081627]\n",
      " [ 0.21207409 10.00378258  3.32470273]]\n",
      "sigmax = \n",
      "[[ 0.27373558 10.01606623  3.8479306 ]\n",
      " [-3.13457853  9.81983973  3.8200271 ]\n",
      " [ 0.1836183  10.70097485  3.83915928]\n",
      " [ 0.33601679  9.93023413  4.34998173]\n",
      " [-2.60113561  9.8341328   3.76929429]\n",
      " [ 0.36385286  9.33037655  3.85648186]\n",
      " [ 0.21145438 10.10152513  3.34577432]]\n",
      "sqP = \n",
      "[[1.4392136491210215, 0.0, 0.0], [0.06598579478494793, 0.5670898560303302, 0.0], [0.026172789767869267, -0.021376262227979104, 0.5124408997236981]]\n",
      "m = \n",
      "[-0.5202198   9.94135115  3.73836886]\n",
      "P = \n",
      "[[ 2.0678678   0.07065244  0.02624571]\n",
      " [ 0.07065244  0.11774768 -0.0119983 ]\n",
      " [ 0.02624571 -0.0119983   0.06283871]]\n",
      "num = \n",
      "1021\n",
      "SENSOR 8 5.12480337929 -1.5292251446\n",
      "\n",
      "num = \n",
      "1022\n",
      "SENSOR 4 1.8502364216 -0.640053414064\n",
      "\n",
      "num = \n",
      "1023\n",
      "SENSOR 5 1.22548414583 3.00267777115\n",
      "\n",
      "num = \n",
      "1024\n",
      "SENSOR 6 4.59119051094 -3.00286235802\n",
      "\n",
      "num = \n",
      "1025\n",
      "ODOMETRY 0.000276055660063 0.0999750533485 0.000328587305689\n",
      "\n",
      "sqP = \n",
      "[[1.4380082743271563, 0.0, 0.0], [0.049132152938055285, 0.3396081806102661, 0.0], [0.01825143117636557, -0.03797032940431529, 0.2471110005033742]]\n",
      "sigma = \n",
      "[[-0.5202198   9.94135115  3.73836886]\n",
      " [ 2.35579675  9.94135115  3.73836886]\n",
      " [-0.42195549 10.62056751  3.73836886]\n",
      " [-0.48371694  9.86541049  4.23259086]\n",
      " [-3.39623635  9.94135115  3.73836886]\n",
      " [-0.61848411  9.26213478  3.73836886]\n",
      " [-0.55672266 10.0172918   3.24414686]]\n",
      "sigmax = \n",
      "[[-0.52105297 10.02803274  3.6887226 ]\n",
      " [ 2.35496358  9.8707389   3.8090255 ]\n",
      " [-0.42278866 10.71170156  3.6974661 ]\n",
      " [-0.48455011  9.95384617  4.1861411 ]\n",
      " [ 2.88611579  9.84467806  3.76352369]\n",
      " [-0.61931728  9.34352762  3.68045809]\n",
      " [-0.55755583 10.10210383  3.19137025]]\n",
      "sqP = \n",
      "[[1.439072817919491, 0.0, 0.0], [-0.05904643304476786, 0.5659971931312178, 0.0], [0.04214292220838184, -0.01468274528280338, 0.5116492416397401]]\n",
      "m = \n",
      "[ 0.25814775 10.00256084  3.65774239]\n",
      "P = \n",
      "[[ 2.06746199 -0.06349212  0.04517886]\n",
      " [-0.06349212  0.11496558 -0.01470822]\n",
      " [ 0.04517886 -0.01470822  0.06376272]]\n",
      "num = \n",
      "1026\n",
      "SENSOR 8 4.85707120873 -1.55643745133\n",
      "\n",
      "num = \n",
      "1027\n",
      "SENSOR 4 1.6853612932 -0.640165672521\n",
      "\n",
      "num = \n",
      "1028\n",
      "SENSOR 5 1.40741969379 3.08301703619\n",
      "\n",
      "num = \n",
      "1029\n",
      "SENSOR 6 4.43722471141 -2.92421694343\n",
      "\n",
      "num = \n",
      "1030\n",
      "ODOMETRY -0.000612253935663 0.100030803164 0.000384082346755\n",
      "\n",
      "sqP = \n",
      "[[1.4378671665364133, 0.0, 0.0], [-0.04415715356635253, 0.3361781170166022, 0.0], [0.03142074844090493, -0.039624132897759644, 0.24739721092723937]]\n",
      "sigma = \n",
      "[[ 0.25814775 10.00256084  3.65774239]\n",
      " [ 3.13388208 10.00256084  3.65774239]\n",
      " [ 0.16983344 10.67491708  3.65774239]\n",
      " [ 0.32098925  9.92331258  4.15253681]\n",
      " [-2.61758658 10.00256084  3.65774239]\n",
      " [ 0.34646206  9.33020461  3.65774239]\n",
      " [ 0.19530625 10.08180911  3.16294797]]\n",
      "sigmax = \n",
      "[[ 0.25875239 10.09921613  3.68329172]\n",
      " [ 3.13448673  9.90258855  3.65848565]\n",
      " [ 0.17043809 10.77344912  3.67466719]\n",
      " [ 0.32159389 10.01817257  4.18410567]\n",
      " [-2.61698194  9.91601409  3.60769571]\n",
      " [ 0.3470667   9.42422977  3.6917171 ]\n",
      " [ 0.1959109  10.17987811  3.1823769 ]]\n",
      "sqP = \n",
      "[[1.506784359570779, 0.0, 0.0], [-0.013930780555128692, 0.5674286547169887, 0.0], [0.017463867427864623, -0.016878066462036777, 0.5126181913713491]]\n",
      "m = \n",
      "[0.26214896 9.82613557 3.60640253]\n",
      "P = \n",
      "[[ 2.27005176 -0.01581538  0.01969085]\n",
      " [-0.01581538  0.10934551 -0.01131407]\n",
      " [ 0.01969085 -0.01131407  0.0631215 ]]\n",
      "num = \n",
      "1031\n",
      "SENSOR 8 4.8148264003 -1.5861711641\n",
      "\n",
      "num = \n",
      "1032\n",
      "SENSOR 4 1.62945141541 -0.645906242858\n",
      "\n",
      "num = \n",
      "1033\n",
      "SENSOR 5 1.57411900279 3.00136290051\n",
      "\n",
      "num = \n",
      "1034\n",
      "SENSOR 6 4.74165161304 -2.88381335457\n",
      "\n",
      "num = \n",
      "1035\n",
      "ODOMETRY -0.000615965348572 0.0999645830188 0.000122066793836\n",
      "\n",
      "sqP = \n",
      "[[1.5066690930514977, 0.0, 0.0], [-0.010496919175029932, 0.33050766983642327, 0.0], [0.013069127290867589, -0.033817314561268845, 0.2486103021591405]]\n",
      "sigma = \n",
      "[[ 0.26214896  9.82613557  3.60640253]\n",
      " [ 3.27548714  9.82613557  3.60640253]\n",
      " [ 0.24115512 10.48715091  3.60640253]\n",
      " [ 0.28828721  9.75850094  4.10362313]\n",
      " [-2.75118923  9.82613557  3.60640253]\n",
      " [ 0.28314279  9.16512023  3.60640253]\n",
      " [ 0.2360107   9.8937702   3.10918192]]\n",
      "sigmax = \n",
      "[[ 0.26192078  9.92276471  3.63226702]\n",
      " [-3.00792634  9.72699194  3.59310964]\n",
      " [ 0.24092695 10.58430171  3.63023286]\n",
      " [ 0.28805904  9.8544211   4.13200422]\n",
      " [-2.7514174   9.73360824  3.5683913 ]\n",
      " [ 0.28291462  9.26118512  3.63428979]\n",
      " [ 0.23578253  9.9910423   3.13251215]]\n",
      "sqP = \n",
      "[[1.433507626318098, 0.0, 0.0], [0.07641946556488946, 0.5582906207691526, 0.0], [0.023175943155070194, -0.015749066640618318, 0.5120430705596127]]\n",
      "m = \n",
      "[-0.54236162  9.79693686  3.45264612]\n",
      "P = \n",
      "[[ 2.05065237  0.08202032  0.02263624]\n",
      " [ 0.08202032  0.11816352 -0.01183791]\n",
      " [ 0.02263624 -0.01183791  0.06230593]]\n",
      "num = \n",
      "1036\n",
      "SENSOR 8 5.09246076865 -1.62935789257\n",
      "\n",
      "num = \n",
      "1037\n",
      "SENSOR 4 1.50283498391 -0.764000413422\n",
      "\n",
      "num = \n",
      "1038\n",
      "SENSOR 5 1.65903158666 3.20124071317\n",
      "\n",
      "num = \n",
      "1039\n",
      "SENSOR 6 4.68619938405 -3.0067714176\n",
      "\n",
      "num = \n",
      "1040\n",
      "ODOMETRY 0.00010181891152 0.100046833019 0.000440816980011\n",
      "\n",
      "sqP = \n",
      "[[1.4320099051838302, 0.0, 0.0], [0.05727636307285968, 0.33894385193325705, 0.0], [0.015807318064075582, -0.03759706849145205, 0.246257027583673]]\n",
      "sigma = \n",
      "[[-0.54236162  9.79693686  3.45264612]\n",
      " [ 2.32165819  9.79693686  3.45264612]\n",
      " [-0.4278089  10.47482457  3.45264612]\n",
      " [-0.51074699  9.72174273  3.94516017]\n",
      " [-3.40638143  9.79693686  3.45264612]\n",
      " [-0.65691435  9.11904916  3.45264612]\n",
      " [-0.57397626  9.872131    2.96013206]]\n",
      "sigmax = \n",
      "[[-0.54285552  9.88252394  3.40099565]\n",
      " [ 2.32116429  9.72877915  3.52577233]\n",
      " [-0.42830279 10.56575448  3.41111697]\n",
      " [-0.51124089  9.80891967  3.89624087]\n",
      " [ 2.87630998  9.7004724   3.47886681]\n",
      " [-0.65740825  9.19817153  3.39155137]\n",
      " [-0.57447016  9.95604267  2.90580205]]\n",
      "sqP = \n",
      "[[1.4399107434487364, 0.0, 0.0], [-0.056088654328952756, 0.5660588603424028, 0.0], [0.043183179072198576, -0.014330677680745474, 0.5111968563181134]]\n",
      "m = \n",
      "[0.23032311 9.90986553 3.36109939]\n",
      "P = \n",
      "[[ 2.07005183 -0.06055568  0.04640351]\n",
      " [-0.06055568  0.11545299 -0.01560678]\n",
      " [ 0.04640351 -0.01560678  0.0641309 ]]\n",
      "num = \n",
      "1041\n",
      "SENSOR 8 5.10307136294 -1.5981894044\n",
      "\n",
      "num = \n",
      "1042\n",
      "SENSOR 4 1.3396692961 -0.647480763035\n",
      "\n",
      "num = \n",
      "1043\n",
      "SENSOR 5 1.89463338292 3.13168562413\n",
      "\n",
      "num = \n",
      "1044\n",
      "SENSOR 6 4.79308799565 -3.00728766374\n",
      "\n",
      "num = \n",
      "1045\n",
      "ODOMETRY -0.000291816369108 0.100048609814 -0.00104644833934\n",
      "\n",
      "sqP = \n",
      "[[1.4387674695855357, 0.0, 0.0], [-0.04208858354550162, 0.33716693826120986, 0.0], [0.032252264318960444, -0.04226193219132407, 0.24759770038247186]]\n",
      "sigma = \n",
      "[[ 0.23032311  9.90986553  3.36109939]\n",
      " [ 3.10785805  9.90986553  3.36109939]\n",
      " [ 0.14614594 10.58419941  3.36109939]\n",
      " [ 0.29482763  9.82534167  3.85629479]\n",
      " [-2.64721183  9.90986553  3.36109939]\n",
      " [ 0.31450027  9.23553166  3.36109939]\n",
      " [ 0.16581858  9.9943894   2.86590399]]\n",
      "sigmax = \n",
      "[[ 0.23086574 10.00726807  3.38394921]\n",
      " [ 3.10840068  9.80987528  3.36446361]\n",
      " [ 0.14668857 10.68317823  3.37567892]\n",
      " [ 0.29537027  9.92106874  3.88537564]\n",
      " [-2.6466692   9.82180287  3.31361954]\n",
      " [ 0.31504291  9.33066815  3.3920577 ]\n",
      " [ 0.16636121 10.09306226  2.88242774]]\n",
      "sqP = \n",
      "[[1.5076019000240524, 0.0, 0.0], [-0.013205317928335961, 0.5682177941918447, 0.0], [0.01760900974735673, -0.01823664152479884, 0.5126227906516027]]\n",
      "m = \n",
      "[0.22595529 9.93256448 3.20769889]\n",
      "P = \n",
      "[[ 2.27252517 -0.01496057  0.01988557]\n",
      " [-0.01496057  0.10873134 -0.01157487]\n",
      " [ 0.01988557 -0.01157487  0.06402832]]\n",
      "num = \n",
      "1046\n",
      "SENSOR 8 5.05308894221 -1.61739005656\n",
      "\n",
      "num = \n",
      "1047\n",
      "SENSOR 4 1.55728242344 -0.778699498829\n",
      "\n",
      "num = \n",
      "1048\n",
      "SENSOR 5 1.76096780816 3.0522097073\n",
      "\n",
      "num = \n",
      "1049\n",
      "ODOMETRY 8.52183257962e-05 0.0998196908854 0.000109532441973\n",
      "\n",
      "sqP = \n",
      "[[1.507489692232666, 0.0, 0.0], [-0.00992416029291942, 0.3295949802938319, 0.0], [0.013191183911022674, -0.03472127532275459, 0.25029732179695496]]\n",
      "sigma = \n",
      "[[ 0.22595529  9.93256448  3.20769889]\n",
      " [ 3.24093467  9.93256448  3.20769889]\n",
      " [ 0.20610697 10.59175444  3.20769889]\n",
      " [ 0.25233766  9.86312192  3.70829353]\n",
      " [-2.78902409  9.93256448  3.20769889]\n",
      " [ 0.24580361  9.27337451  3.20769889]\n",
      " [ 0.19957292 10.00200703  2.70710424]]\n",
      "sigmax = \n",
      "[[ 0.22461703 10.03007644  3.23008507]\n",
      " [-3.0435889   9.83300625  3.19780525]\n",
      " [ 0.2047687  10.68969149  3.22814534]\n",
      " [ 0.25099939  9.96000942  3.73324422]\n",
      " [-2.79036236  9.83865989  3.17317855]\n",
      " [ 0.24446535  9.37042297  3.23201598]\n",
      " [ 0.19823466 10.10007559  2.72691034]]\n",
      "sqP = \n",
      "[[1.4334681899066837, 0.0, 0.0], [0.07726907778708081, 0.5577724990496912, 0.0], [0.020372742728469947, -0.016293029574586095, 0.5128599785748855]]\n",
      "m = \n",
      "[-0.56177537  9.97164305  3.22735492]\n",
      "P = \n",
      "[[ 2.0504841   0.08235301  0.01988238]\n",
      " [ 0.08235301  0.11635462 -0.02117648]\n",
      " [ 0.01988238 -0.02117648  0.09535234]]\n",
      "num = \n",
      "1050\n",
      "SENSOR 8 4.93860295592 -1.59049668067\n",
      "\n",
      "num = \n",
      "1051\n",
      "SENSOR 4 1.44172934018 -0.934292188086\n",
      "\n",
      "num = \n",
      "1052\n",
      "SENSOR 5 1.92234124612 3.05518288622\n",
      "\n",
      "num = \n",
      "1053\n",
      "ODOMETRY 7.22338246953e-05 0.0999023454001 -3.77129042551e-05\n",
      "\n",
      "sqP = \n",
      "[[1.431951152723178, 0.0, 0.0], [0.05751104522369759, 0.336224777554241, 0.0], [0.013884815367586283, -0.06535808629797776, 0.3014761576596053]]\n",
      "sigma = \n",
      "[[-0.56177537  9.97164305  3.22735492]\n",
      " [ 2.30212693  9.97164305  3.22735492]\n",
      " [-0.44675328 10.6440926   3.22735492]\n",
      " [-0.53400574  9.84092687  3.83030723]\n",
      " [-3.42567768  9.97164305  3.22735492]\n",
      " [-0.67679746  9.29919349  3.22735492]\n",
      " [-0.589545   10.10235922  2.6244026 ]]\n",
      "sigmax = \n",
      "[[-0.56158062 10.05612605  3.17418921]\n",
      " [ 2.30232168  9.90497108  3.30164367]\n",
      " [-0.44655853 10.73411912  3.18423651]\n",
      " [-0.53381099  9.92685351  3.77950778]\n",
      " [ 2.85770238  9.87582189  3.25532414]\n",
      " [-0.67660271  9.3770165   3.16484451]\n",
      " [-0.58935025 10.18533345  2.56891163]]\n",
      "sqP = \n",
      "[[1.4399059399793355, 0.0, 0.0], [-0.055492312796421604, 0.5669172485976137, 0.0], [0.0446104649068716, -0.03278372886115185, 0.5391881444604895]]\n",
      "m = \n",
      "[0.22814254 9.90910068 3.12226171]\n",
      "P = \n",
      "[[ 2.07026174 -0.05964985  0.05010661]\n",
      " [-0.05964985  0.11350519 -0.02622798]\n",
      " [ 0.05010661 -0.02622798  0.10320402]]\n",
      "num = \n",
      "1054\n",
      "SENSOR 8 5.1479288931 -1.65822977182\n",
      "\n",
      "num = \n",
      "1055\n",
      "SENSOR 4 1.25455156912 -0.879623084747\n",
      "\n",
      "num = \n",
      "1056\n",
      "SENSOR 5 2.37861473612 3.10537197028\n",
      "\n",
      "num = \n",
      "1057\n",
      "ODOMETRY 0.000670127696266 0.100006079133 0.000625609916127\n",
      "\n",
      "sqP = \n",
      "[[1.438840416610967, 0.0, 0.0], [-0.04145688824532957, 0.3343449122112488, 0.0], [0.03482430142614024, -0.07412785562337049, 0.31063860852006225]]\n",
      "sigma = \n",
      "[[ 0.22814254  9.90910068  3.12226171]\n",
      " [ 3.10582337  9.90910068  3.12226171]\n",
      " [ 0.14522876 10.57779051  3.12226171]\n",
      " [ 0.29779114  9.76084497  3.74353893]\n",
      " [-2.64953829  9.90910068  3.12226171]\n",
      " [ 0.31105632  9.24041086  3.12226171]\n",
      " [ 0.15849394 10.05735639  2.5009845 ]]\n",
      "sigmax = \n",
      "[[ 0.22817706 10.00641274  3.14486352]\n",
      " [ 3.10585789  9.80926198  3.12582718]\n",
      " [ 0.14526328 10.67664012  3.1367266 ]\n",
      " [ 0.29782566  9.85634819  3.77285811]\n",
      " [-2.64950377  9.82105374  3.07505774]\n",
      " [ 0.31109084  9.33551676  3.15284514]\n",
      " [ 0.15852846 10.15600543  2.51675933]]\n",
      "sqP = \n",
      "[[1.5077112289911572, 0.0, 0.0], [-0.01376268525865916, 0.5698153372521365, 0.0], [0.019476558903047426, -0.040461492055371785, 0.5451069804199601]]\n",
      "m = \n",
      "[ 0.21580225 10.014492    2.80896487]\n",
      "P = \n",
      "[[ 2.27284658 -0.01565006  0.02278251]\n",
      " [-0.01565006  0.11152642 -0.02488501]\n",
      " [ 0.02278251 -0.02488501  0.10408411]]\n",
      "num = \n",
      "1058\n",
      "SENSOR 8 5.09480518246 -1.65212699893\n",
      "\n",
      "num = \n",
      "1059\n",
      "SENSOR 4 1.40390131939 -0.879816536972\n",
      "\n",
      "num = \n",
      "1060\n",
      "SENSOR 5 2.20542921923 3.11477051951\n",
      "\n",
      "num = \n",
      "1061\n",
      "ODOMETRY 0.000570783669274 0.0999546481828 0.000310239449538\n",
      "\n",
      "sqP = \n",
      "[[1.5075962912249607, 0.0, 0.0], [-0.01038080605851342, 0.333794341025638, 0.0], [0.015111812356268907, -0.07408194718022103, 0.31363610973474304]]\n",
      "sigma = \n",
      "[[ 0.21580225 10.014492    2.80896487]\n",
      " [ 3.23099483 10.014492    2.80896487]\n",
      " [ 0.19504064 10.68208069  2.80896487]\n",
      " [ 0.24602587  9.86632811  3.43623709]\n",
      " [-2.79939033 10.014492    2.80896487]\n",
      " [ 0.23656386  9.34690332  2.80896487]\n",
      " [ 0.18557862 10.1626559   2.18169265]]\n",
      "sigmax = \n",
      "[[ 0.21709799 10.11216406  2.83044475]\n",
      " [-3.05089474  9.91489133  2.79996927]\n",
      " [ 0.19633637 10.78017762  2.82841243]\n",
      " [ 0.24732161  9.96330646  3.46065871]\n",
      " [-2.7980946   9.92030699  2.77534346]\n",
      " [ 0.2378596   9.44410841  2.8324678 ]\n",
      " [ 0.18687436 10.26093245  2.20021116]]\n",
      "sqP = \n",
      "[[1.4334856810262917, 0.0, 0.0], [0.07679817045972616, 0.5641844290265154, 0.0], [0.020638469863890802, -0.0422162238956092, 0.5454076286453627]]\n",
      "m = \n",
      "[-0.57136332 10.03874177  2.82610837]\n",
      "P = \n",
      "[[ 2.05050342  0.0808214   0.02144483]\n",
      " [ 0.0808214   0.10673943 -0.01533132]\n",
      " [ 0.02144483 -0.01533132  0.11199331]]\n",
      "num = \n",
      "1062\n",
      "SENSOR 4 1.06631403361 -1.08193342846\n",
      "\n",
      "num = \n",
      "1063\n",
      "SENSOR 5 2.47203994196 3.13761545266\n",
      "\n",
      "num = \n",
      "1064\n",
      "ODOMETRY 3.85114033074e-05 0.100176546577 0.000173947982474\n",
      "\n",
      "sqP = \n",
      "[[1.431957896566492, 0.0, 0.0], [0.056441186998265044, 0.3217978047953928, 0.0], [0.014975881918110223, -0.0502693774496197, 0.3305178073401708]]\n",
      "sigma = \n",
      "[[-0.57136332 10.03874177  2.82610837]\n",
      " [ 2.29255248 10.03874177  2.82610837]\n",
      " [-0.45848094 10.68233738  2.82610837]\n",
      " [-0.54141155  9.93820301  3.48714398]\n",
      " [-3.43527911 10.03874177  2.82610837]\n",
      " [-0.68424569  9.39514616  2.82610837]\n",
      " [-0.60131508 10.13928052  2.16507275]]\n",
      "sigmax = \n",
      "[[-0.57048229 10.12285091  2.77210296]\n",
      " [ 2.2934335   9.97265852  2.90110128]\n",
      " [-0.45759992 10.77199453  2.78192096]\n",
      " [-0.54053053 10.02389174  3.43568164]\n",
      " [ 2.84878722  9.94305035  2.8549889 ]\n",
      " [-0.68336467  9.47263667  2.76297239]\n",
      " [-0.60043406 10.22173462  2.10857273]]\n",
      "sqP = \n",
      "[[1.43987389015203, 0.0, 0.0], [-0.05591636157721429, 0.5567655641254575, 0.0], [0.045815336042880475, -0.027828462765495034, 0.5562891306562593]]\n",
      "m = \n",
      "[0.21981623 9.89511048 2.62004535]\n",
      "P = \n",
      "[[ 2.07121112 -0.07041237  0.05289094]\n",
      " [-0.07041237  0.19812941 -0.03677821]\n",
      " [ 0.05289094 -0.03677821  0.11891653]]\n",
      "num = \n",
      "1065\n",
      "SENSOR 4 1.20265396615 -1.09036457216\n",
      "\n",
      "num = \n",
      "1066\n",
      "SENSOR 5 2.34144827576 3.04411849412\n",
      "\n",
      "num = \n",
      "1067\n",
      "ODOMETRY 0.000776427377761 0.099952687123 -0.000941767872499\n",
      "\n",
      "sqP = \n",
      "[[1.4391702887390372, 0.0, 0.0], [-0.04892566816481034, 0.44242025859261364, 0.0], [0.03675099655109914, -0.07906541691762155, 0.33363835126565883]]\n",
      "sigma = \n",
      "[[ 0.21981623  9.89511048  2.62004535]\n",
      " [ 3.0981568   9.89511048  2.62004535]\n",
      " [ 0.12196489 10.779951    2.62004535]\n",
      " [ 0.29331822  9.73697964  3.28732205]\n",
      " [-2.65852435  9.89511048  2.62004535]\n",
      " [ 0.31766756  9.01026996  2.62004535]\n",
      " [ 0.14631423 10.05324131  1.95276865]]\n",
      "sigmax = \n",
      "[[ 0.22002869  9.99287569  2.64189264]\n",
      " [ 3.09836926  9.79502825  2.62439138]\n",
      " [ 0.12217735 10.87938291  2.63223693]\n",
      " [ 0.29353068  9.83287651  3.31628982]\n",
      " [-2.65831189  9.80639854  2.57351007]\n",
      " [ 0.31788002  9.10543312  2.65133933]\n",
      " [ 0.14652669 10.15234693  1.96737748]]\n",
      "sqP = \n",
      "[[1.5082955535113387, 0.0, 0.0], [-0.01904406534172429, 0.6400089905599958, 0.0], [0.02050913351329314, -0.04242721878915522, 0.5585978672639602]]\n",
      "m = \n",
      "[0.22437222 9.85141073 2.67665478]\n",
      "P = \n",
      "[[ 2.2746444  -0.02504513  0.02508256]\n",
      " [-0.02504513  0.26037691 -0.04996355]\n",
      " [ 0.02508256 -0.04996355  0.12506879]]\n",
      "num = \n",
      "1068\n",
      "SENSOR 4 1.16717333388 -1.12821635833\n",
      "\n",
      "num = \n",
      "1069\n",
      "SENSOR 5 2.46018754147 3.12942300941\n",
      "\n",
      "num = \n",
      "1070\n",
      "ODOMETRY -0.000182525437042 0.0999468132926 -0.000158790994297\n",
      "\n",
      "sqP = \n",
      "[[1.5081924275910306, 0.0, 0.0], [-0.01660605882109732, 0.5100011247389886, 0.0], [0.016630874983070587, -0.09742600866687809, 0.33955909847252175]]\n",
      "sigma = \n",
      "[[ 0.22437222  9.85141073  2.67665478]\n",
      " [ 3.24075707  9.85141073  2.67665478]\n",
      " [ 0.1911601  10.87141298  2.67665478]\n",
      " [ 0.25763397  9.65655871  3.35577298]\n",
      " [-2.79201264  9.85141073  2.67665478]\n",
      " [ 0.25758434  8.83140848  2.67665478]\n",
      " [ 0.19111047 10.04626275  1.99753659]]\n",
      "sigmax = \n",
      "[[ 0.22420688  9.9488407   2.69896935]\n",
      " [-3.04259357  9.7519568   2.66668205]\n",
      " [ 0.19099476 10.9695302   2.69572178]\n",
      " [ 0.25746863  9.75319271  3.3813153 ]\n",
      " [-2.79217798  9.75753013  2.64234776]\n",
      " [ 0.25741899  8.92804375  2.7021923 ]\n",
      " [ 0.19094513 10.14438092  2.01659871]]\n",
      "sqP = \n",
      "[[1.4335349387413114, 0.0, 0.0], [0.07275518388977796, 0.6868866627246498, 0.0], [0.021974422894184095, -0.04976351326420663, 0.5602266982472426]]\n",
      "m = \n",
      "[-0.5748333   9.79638699  2.63363426]\n",
      "P = \n",
      "[[ 2.05252346  0.08644456  0.0189989 ]\n",
      " [ 0.08644456  0.32274222 -0.06347228]\n",
      " [ 0.0189989  -0.06347228  0.12701486]]\n",
      "num = \n",
      "1071\n",
      "SENSOR 4 1.0123884597 -1.2839270903\n",
      "\n",
      "num = \n",
      "1072\n",
      "SENSOR 5 2.62303723473 2.94037324821\n",
      "\n",
      "num = \n",
      "1073\n",
      "ODOMETRY -0.000682345311597 0.099958072456 0.00043862067399\n",
      "\n",
      "sqP = \n",
      "[[1.4326630654651469, 0.0, 0.0], [0.06033837697443697, 0.5648906957391233, 0.0], [0.013261244595964767, -0.11377854081808028, 0.337481030854291]]\n",
      "sigma = \n",
      "[[-0.5748333   9.79638699  2.63363426]\n",
      " [ 2.29049283  9.79638699  2.63363426]\n",
      " [-0.45415655 10.92616838  2.63363426]\n",
      " [-0.54831081  9.56882991  3.30859632]\n",
      " [-3.44015943  9.79638699  2.63363426]\n",
      " [-0.69551006  8.6666056   2.63363426]\n",
      " [-0.60135579 10.02394407  1.9586722 ]]\n",
      "sigmax = \n",
      "[[-0.57517462  9.88026073  2.57927838]\n",
      " [ 2.29015151  9.73052012  2.70880687]\n",
      " [-0.45449787 11.01597572  2.58977075]\n",
      " [-0.54865213  9.65411563  3.25648383]\n",
      " [ 2.84268456  9.70086729  2.66305112]\n",
      " [-0.69585137  8.74332577  2.56957662]\n",
      " [-0.60169711 10.10634683  1.90211115]]\n",
      "sqP = \n",
      "[[1.4399472918943912, 0.0, 0.0], [-0.04517784245604011, 0.733941033500157, 0.0], [0.04580425679672419, -0.04978661539565234, 0.5588847198109673]]\n",
      "m = \n",
      "[0.21211532 9.67951492 2.50244175]\n",
      "P = \n",
      "[[ 2.07210978 -0.05797171  0.05415433]\n",
      " [-0.05797171  0.37795414 -0.07700875]\n",
      " [ 0.05415433 -0.07700875  0.13169974]]\n",
      "num = \n",
      "1074\n",
      "SENSOR 4 1.00020435353 -1.2311017102\n",
      "\n",
      "num = \n",
      "1075\n",
      "SENSOR 5 2.83365938044 3.10650495611\n",
      "\n",
      "num = \n",
      "1076\n",
      "ODOMETRY -9.19632135689e-07 0.10002313352 -0.000224880476229\n",
      "\n",
      "sqP = \n",
      "[[1.4394824700062658, 0.0, 0.0], [-0.04027260611754097, 0.6134592541739092, 0.0], [0.03762069321678452, -0.12306223651184572, 0.33932301952803473]]\n",
      "sigma = \n",
      "[[ 0.21211532  9.67951492  2.50244175]\n",
      " [ 3.09108026  9.67951492  2.50244175]\n",
      " [ 0.13157011 10.90643343  2.50244175]\n",
      " [ 0.28735671  9.43339045  3.18108779]\n",
      " [-2.66684962  9.67951492  2.50244175]\n",
      " [ 0.29266053  8.45259641  2.50244175]\n",
      " [ 0.13687393  9.92563939  1.82379571]]\n",
      "sigmax = \n",
      "[[ 0.2118716   9.77724705  2.52341907]\n",
      " [ 3.09083654  9.57968781  2.50755684]\n",
      " [ 0.13132638 11.0055365   2.51548771]\n",
      " [ 0.28711298  9.52926919  3.20935232]\n",
      " [-2.66709334  9.59058003  2.4568106 ]\n",
      " [ 0.29241681  8.54832389  2.5312144 ]\n",
      " [ 0.13663021 10.02467188  1.83736711]]\n",
      "sqP = \n",
      "[[1.5083590357792638, 0.0, 0.0], [-0.022089388213571697, 0.7740366572097442, 0.0], [0.02076705809576621, -0.05553882071890223, 0.5609146097674726]]\n",
      "m = \n",
      "[0.20994424 9.61938087 2.36518427]\n",
      "P = \n",
      "[[ 2.27490861 -0.0308454   0.02641791]\n",
      " [-0.0308454   0.45674411 -0.09035329]\n",
      " [ 0.02641791 -0.09035329  0.13553173]]\n",
      "num = \n",
      "1077\n",
      "SENSOR 4 0.849352538956 -1.46997822321\n",
      "\n",
      "num = \n",
      "1078\n",
      "SENSOR 5 2.85184515953 3.11408226771\n",
      "\n",
      "num = \n",
      "1079\n",
      "ODOMETRY -0.000798214396223 0.0999079224924 0.000213586041509\n",
      "\n",
      "sqP = \n",
      "[[1.508280018130901, 0.0, 0.0], [-0.020450710277692703, 0.6755189686766986, 0.0], [0.01751525627184414, -0.1332236312704576, 0.34274831606918055]]\n",
      "sigma = \n",
      "[[ 0.20994424  9.61938087  2.36518427]\n",
      " [ 3.22650428  9.61938087  2.36518427]\n",
      " [ 0.16904282 10.97041881  2.36518427]\n",
      " [ 0.24497475  9.35293361  3.0506809 ]\n",
      " [-2.8066158   9.61938087  2.36518427]\n",
      " [ 0.25084566  8.26834293  2.36518427]\n",
      " [ 0.17491373  9.88582813  1.67968764]]\n",
      "sigmax = \n",
      "[[ 0.20971844  9.71720777  2.38602953]\n",
      " [-3.05690683  9.51971809  2.35670143]\n",
      " [ 0.16881702 11.06901625  2.38201196]\n",
      " [ 0.24474895  9.44997042  3.0749396 ]\n",
      " [-2.8068416   9.5249172   2.33230202]\n",
      " [ 0.25061986  8.36523565  2.39001224]\n",
      " [ 0.17468793  9.98432508  1.69709389]]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "sqP = \n",
      "[[1.4335913398895648, 0.0, 0.0], [0.06884093428020804, 0.822878237821597, 0.0], [0.02103030080014706, -0.057489656107853886, 0.5614897579908795]]\n",
      "m = \n",
      "[-0.59291826  9.50686732  2.29478933]\n",
      "P = \n",
      "[[ 2.05341728  0.0844645   0.01761555]\n",
      " [ 0.0844645   0.54023611 -0.09750079]\n",
      " [ 0.01761555 -0.09750079  0.14055173]]\n",
      "num = \n",
      "1080\n",
      "SENSOR 4 1.10312028502 -1.48363601494\n",
      "\n",
      "num = \n",
      "1081\n",
      "SENSOR 5 3.00416166275 3.10884116279\n",
      "\n",
      "num = \n",
      "1082\n",
      "ODOMETRY -0.000984233781833 0.0999672948533 7.64590930276e-05\n",
      "\n",
      "sqP = \n",
      "[[1.432974974939192, 0.0, 0.0], [0.05894346092381142, 0.7326402795881762, 0.0], [0.01229299421085207, -0.13407040937690046, 0.3498938893716918]]\n",
      "sigma = \n",
      "[[-0.59291826  9.50686732  2.29478933]\n",
      " [ 2.27303169  9.50686732  2.29478933]\n",
      " [-0.47503134 10.97214787  2.29478933]\n",
      " [-0.56833227  9.2387265   2.99457711]\n",
      " [-3.45886821  9.50686732  2.29478933]\n",
      " [-0.71080518  8.04158676  2.29478933]\n",
      " [-0.61750425  9.77500813  1.59500155]]\n",
      "sigmax = \n",
      "[[-0.59350289  9.58967772  2.23889631]\n",
      " [ 2.27244706  9.44239511  2.37111055]\n",
      " [-0.47561597 11.06095733  2.24902391]\n",
      " [-0.5689169   9.32288592  2.94073675]\n",
      " [ 2.82373247  9.41197082  2.32603429]\n",
      " [-0.71138981  8.1172486   2.22954457]\n",
      " [-0.61808888  9.85641946  1.53708965]]\n",
      "sqP = \n",
      "[[1.4398510337961818, 0.0, 0.0], [-0.038135479932413105, 0.8734541845651411, 0.0], [0.046930380906815124, -0.051153153112413724, 0.5663219593460813]]\n",
      "m = \n",
      "[0.18752799 9.55043972 2.21193153]\n",
      "P = \n",
      "[[ 2.07231384 -0.05528126  0.05650657]\n",
      " [-0.05528126  0.65027389 -0.11430144]\n",
      " [ 0.05650657 -0.11430144  0.14780457]]\n",
      "num = \n",
      "1083\n",
      "SENSOR 4 1.04848307593 -1.64994919996\n",
      "\n",
      "num = \n",
      "1084\n",
      "SENSOR 5 3.02744738541 3.06646084819\n",
      "\n",
      "num = \n",
      "1085\n",
      "ODOMETRY -0.000260628905805 0.0999789088987 0.00116057007669\n",
      "\n",
      "sqP = \n",
      "[[1.4395533456753788, 0.0, 0.0], [-0.03840167334554862, 0.8054807258510596, 0.0], [0.03925285086948799, -0.14003322813304964, 0.35588548076014337]]\n",
      "sigma = \n",
      "[[ 0.18752799  9.55043972  2.21193153]\n",
      " [ 3.06663468  9.55043972  2.21193153]\n",
      " [ 0.11072465 11.16140117  2.21193153]\n",
      " [ 0.2660337   9.27037326  2.92370249]\n",
      " [-2.6915787   9.55043972  2.21193153]\n",
      " [ 0.26433134  7.93947827  2.21193153]\n",
      " [ 0.10902229  9.83050618  1.50016057]]\n",
      "sigmax = \n",
      "[[ 0.18662022  9.64867269  2.23047184]\n",
      " [ 3.06572691  9.45076055  2.21951597]\n",
      " [ 0.10981687 11.26076712  2.22287998]\n",
      " [ 0.26512592  9.36684965  2.94988963]\n",
      " [-2.69248647  9.46038231  2.16853656]\n",
      " [ 0.26342357  8.0359991   2.23795439]\n",
      " [ 0.10811452  9.92989062  1.51093984]]\n",
      "sqP = \n",
      "[[1.508419474106025, 0.0, 0.0], [-0.02648276690355594, 0.9359704539212922, 0.0], [0.021620203269013687, -0.055360902039095314, 0.5712302028921596]]\n",
      "m = \n",
      "[0.18618819 9.55366043 2.17147716]\n",
      "P = \n",
      "[[ 2.2751281  -0.03822147  0.0282717 ]\n",
      " [-0.03822147  0.72872184 -0.12253275]\n",
      " [ 0.0282717  -0.12253275  0.14966014]]\n",
      "num = \n",
      "1086\n",
      "SENSOR 4 1.01403868185 -1.8074118964\n",
      "\n",
      "num = \n",
      "1087\n",
      "SENSOR 5 3.08384570669 3.04030179903\n",
      "\n",
      "num = \n",
      "1088\n",
      "ODOMETRY -0.000220095556118 0.0999551962105 -0.000481994451249\n",
      "\n",
      "sqP = \n",
      "[[1.5083527769704532, 0.0, 0.0], [-0.02533987524706995, 0.8532758825796837, 0.0], [0.018743427742063157, -0.14304611014953694, 0.3589521401155391]]\n",
      "sigma = \n",
      "[[ 0.18618819  9.55366043  2.17147716]\n",
      " [ 3.20289374  9.55366043  2.17147716]\n",
      " [ 0.13550844 11.2602122   2.17147716]\n",
      " [ 0.22367504  9.26756821  2.88938144]\n",
      " [-2.83051737  9.55366043  2.17147716]\n",
      " [ 0.23686794  7.84710867  2.17147716]\n",
      " [ 0.14870133  9.83975265  1.45357288]]\n",
      "sigmax = \n",
      "[[ 0.18708813  9.65191623  2.18995908]\n",
      " [-3.07939162  9.45386772  2.16537819]\n",
      " [ 0.13640838 11.35927809  2.1849579 ]\n",
      " [ 0.22457498  9.36506231  2.91153281]\n",
      " [-2.82961742  9.45847204  2.14090017]\n",
      " [ 0.23776788  7.94430205  2.1949128 ]\n",
      " [ 0.14960127  9.93863208  1.46835938]]\n",
      "sqP = \n",
      "[[1.4336817804101407, 0.0, 0.0], [0.06348426550230996, 0.9760076324768452, 0.0], [0.019617286096055578, -0.05508330414505012, 0.5718832989059713]]\n",
      "m = \n",
      "[-0.60555883  9.52817843  2.12191686]\n",
      "P = \n",
      "[[ 2.05407377  0.07752261  0.01599748]\n",
      " [ 0.07752261  0.79590097 -0.12744259]\n",
      " [ 0.01599748 -0.12744259  0.15166114]]\n",
      "num = \n",
      "1089\n",
      "SENSOR 4 0.949362251022 -1.93475998957\n",
      "\n",
      "num = \n",
      "1090\n",
      "SENSOR 5 3.02874489038 3.02677668221\n",
      "\n",
      "num = \n",
      "1091\n",
      "ODOMETRY 0.00031060131166 0.100181543701 -0.20019265501\n",
      "\n",
      "sqP = \n",
      "[[1.4332040221649653, 0.0, 0.0], [0.05409041876086468, 0.8904915453214151, 0.0], [0.011162038051944738, -0.1437928859836492, 0.36174597753964954]]\n",
      "sigma = \n",
      "[[-0.60555883  9.52817843  2.12191686]\n",
      " [ 2.26084921  9.52817843  2.12191686]\n",
      " [-0.49737799 11.30916152  2.12191686]\n",
      " [-0.58323476  9.24059266  2.84540881]\n",
      " [-3.47196688  9.52817843  2.12191686]\n",
      " [-0.71373967  7.74719534  2.12191686]\n",
      " [-0.62788291  9.8157642   1.3984249 ]]\n",
      "sigmax = \n",
      "[[-0.60626092  9.61034748  2.06500211]\n",
      " [ 2.26014712  9.46456612  2.19901754]\n",
      " [-0.49808008 11.39699531  2.07420662]\n",
      " [-0.58393685  9.3240117   2.79034245]\n",
      " [ 2.81051634  9.43363585  2.15436284]\n",
      " [-0.71444176  7.82273896  2.05646304]\n",
      " [-0.628585    9.89664231  1.33969014]]\n",
      "sqP = \n",
      "[[1.439629407866076, 0.0, 0.0], [-0.03418401106228495, 1.0112898523214842, 0.0], [0.047563322407480915, -0.04905793646343886, 0.5738762541009432]]\n",
      "m = \n",
      "[0.18300604 9.50519682 2.11402964]\n",
      "P = \n",
      "[[ 2.07165255 -0.05029339  0.05701192]\n",
      " [-0.05029339  0.86218531 -0.13307928]\n",
      " [ 0.05701192 -0.13307928  0.15613233]]\n",
      "num = \n",
      "1092\n",
      "SENSOR 4 1.03535256961 -1.66182916184\n",
      "\n",
      "num = \n",
      "1093\n",
      "SENSOR 5 3.32942750619 -2.89908305281\n",
      "\n",
      "num = \n",
      "1094\n",
      "ODOMETRY -6.00078729292e-05 0.100231170262 -0.197349468283\n",
      "\n",
      "sqP = \n",
      "[[1.439323641512655, 0.0, 0.0], [-0.03494237898438685, 0.9278816411117369, 0.0], [0.03961021467227055, -0.14193104195897435, 0.36663188037217703]]\n",
      "sigma = \n",
      "[[ 0.18300604  9.50519682  2.11402964]\n",
      " [ 3.06165332  9.50519682  2.11402964]\n",
      " [ 0.11312128 11.3609601   2.11402964]\n",
      " [ 0.26222647  9.22133474  2.8472934 ]\n",
      " [-2.69564124  9.50519682  2.11402964]\n",
      " [ 0.2528908   7.64943354  2.11402964]\n",
      " [ 0.10378561  9.7890589   1.38076588]]\n",
      "sigmax = \n",
      "[[-0.01687601  9.60369977  2.1322919 ]\n",
      " [ 2.86177127  9.40533272  2.12199854]\n",
      " [-0.08676077 11.46049783  2.12536907]\n",
      " [ 0.06234442  9.31808352  2.87329367]\n",
      " [-2.8955233   9.41482638  2.07079162]\n",
      " [ 0.05300875  7.74642083  2.13912557]\n",
      " [-0.09609644  9.88869815  1.39117557]]\n",
      "sqP = \n",
      "[[1.5081254868122616, 0.0, 0.0], [-0.02752511493978585, 1.043437556008761, 0.0], [0.02202917322970739, -0.05191876207613684, 0.5783840852622437]]\n",
      "m = \n",
      "[-0.01931792  9.4476535   1.975536  ]\n",
      "P = \n",
      "[[ 2.27427223 -0.04051881  0.02918928]\n",
      " [-0.04051881  0.92827432 -0.14607543]\n",
      " [ 0.02918928 -0.14607543  0.15736556]]\n",
      "num = \n",
      "1095\n",
      "SENSOR 4 1.23205472755 -1.61328765107\n",
      "\n",
      "num = \n",
      "1096\n",
      "SENSOR 5 3.58883871764 -2.79036998463\n",
      "\n",
      "num = \n",
      "1097\n",
      "ODOMETRY 0.000778944546602 0.0997443640523 -0.196805293229\n",
      "\n",
      "sqP = \n",
      "[[1.5080690395938487, 0.0, 0.0], [-0.02686800859679116, 0.9630952358397411, 0.0], [0.019355397891028418, -0.15113290854113318, 0.3662646252232391]]\n",
      "sigma = \n",
      "[[-0.01931792  9.4476535   1.975536  ]\n",
      " [ 2.99682016  9.4476535   1.975536  ]\n",
      " [-0.07305394 11.37384397  1.975536  ]\n",
      " [ 0.01939288  9.14538768  2.70806525]\n",
      " [-3.035456    9.4476535   1.975536  ]\n",
      " [ 0.0344181   7.52146302  1.975536  ]\n",
      " [-0.05802871  9.74991931  1.24300675]]\n",
      "sigmax = \n",
      "[[-0.21672739  9.54786585  1.97359385]\n",
      " [ 2.79941068  9.34847174  1.99000203]\n",
      " [-0.27046341 11.47380736  1.96821423]\n",
      " [-0.1780166   9.24560012  2.71000289]\n",
      " [ 3.05031983  9.34798571  1.96492374]\n",
      " [-0.16299138  7.62163504  1.97897908]\n",
      " [-0.25543819  9.84998143  1.23718772]]\n",
      "sqP = \n",
      "[[1.4337301207261202, 0.0, 0.0], [-0.10208337664350733, 1.07113402462011, 0.0], [0.006335906741556822, -0.053889419606295855, 0.5767834716371258]]\n",
      "m = \n",
      "[0.57332822 9.44508277 1.79365184]\n",
      "P = \n",
      "[[ 2.05418958 -0.13156371  0.0175782 ]\n",
      " [-0.13156371  1.00046651 -0.15130574]\n",
      " [ 0.0175782  -0.15130574  0.16116719]]\n",
      "num = \n",
      "1098\n",
      "SENSOR 4 0.980807526601 -1.37607808602\n",
      "\n",
      "num = \n",
      "1099\n",
      "SENSOR 5 3.44799435666 -2.62665666687\n",
      "\n",
      "num = \n",
      "1100\n",
      "ODOMETRY -0.000368902911915 0.100159655151 -0.19959086214\n",
      "\n",
      "sqP = \n",
      "[[1.4332444258954269, 0.0, 0.0], [-0.09179432603531235, 0.9960122036944014, 0.0], [0.012264623910818671, -0.15078119692967618, 0.37186260453959086]]\n",
      "sigma = \n",
      "[[ 0.57332822  9.44508277  1.79365184]\n",
      " [ 3.43981708  9.44508277  1.79365184]\n",
      " [ 0.38973957 11.43710718  1.79365184]\n",
      " [ 0.59785747  9.14352038  2.53737705]\n",
      " [-2.29316063  9.44508277  1.79365184]\n",
      " [ 0.75691688  7.45305837  1.79365184]\n",
      " [ 0.54879898  9.74664517  1.04992663]]\n",
      "sigmax = \n",
      "[[ 0.37730188  9.52883587  1.84782154]\n",
      " [-3.03939458  9.34976401  1.76427035]\n",
      " [ 0.19371322 11.52934197  1.83162132]\n",
      " [ 0.40183112  9.22591967  2.59358465]\n",
      " [-2.48918698  9.37919407  1.71876782]\n",
      " [ 0.56089053  7.52551481  1.8622011 ]\n",
      " [ 0.35277263  9.83170168  1.10202584]]\n",
      "sqP = \n",
      "[[1.44154360667212, 0.0, 0.0], [0.0026397571143958734, 1.109023579870183, 0.0], [0.04588536041724492, -0.05506865467811625, 0.5798303091839068]]\n",
      "m = \n",
      "[-0.41152263  9.39820043  1.7805156 ]\n",
      "P = \n",
      "[[ 2.07691739 -0.00385893  0.05212975]\n",
      " [-0.00385893  1.05482636 -0.15227273]\n",
      " [ 0.05212975 -0.15227273  0.16747153]]\n",
      "num = \n",
      "1101\n",
      "SENSOR 4 1.30954404554 -1.43176636885\n",
      "\n",
      "num = \n",
      "1102\n",
      "SENSOR 5 3.51128233361 -2.49583044536\n",
      "\n",
      "num = \n",
      "1103\n",
      "ODOMETRY 0.000875408598912 0.10049913959 -0.198612822628\n",
      "\n",
      "sqP = \n",
      "[[1.4411514114808956, 0.0, 0.0], [-0.0026776730837091644, 1.0270439078986402, 0.0], [0.03617229261814556, -0.1481688094416881, 0.3797487275090729]]\n",
      "sigma = \n",
      "[[-0.41152263  9.39820043  1.7805156 ]\n",
      " [ 2.47078019  9.39820043  1.7805156 ]\n",
      " [-0.41687798 11.45228825  1.7805156 ]\n",
      " [-0.33917804  9.10186281  2.54001305]\n",
      " [-3.29382545  9.39820043  1.7805156 ]\n",
      " [-0.40616728  7.34411262  1.7805156 ]\n",
      " [-0.48386721  9.69453805  1.02101814]]\n",
      "sigmax = \n",
      "[[-0.61148239  9.48998325  1.74041734]\n",
      " [ 2.27082043  9.31976666  1.84280604]\n",
      " [-0.61683774 11.54385501  1.73992639]\n",
      " [-0.53913781  9.19630392  2.50665388]\n",
      " [ 2.78940009  9.29920474  1.79574088]\n",
      " [-0.60612705  7.43610886  1.74090944]\n",
      " [-0.68382698  9.78318243  0.97439057]]\n",
      "sqP = \n",
      "[[1.4382886476494992, 0.0, 0.0], [-0.07995464766082212, 1.1295406933763958, 0.0], [0.04085478073553258, -0.04939620265438641, 0.5864586324174149]]\n",
      "m = \n",
      "[0.1688518  9.50309188 1.74543803]\n",
      "P = \n",
      "[[ 2.06798216 -0.10905117  0.05450456]\n",
      " [-0.10905117  1.10854774 -0.15873252]\n",
      " [ 0.05450456 -0.15873252  0.17047037]]\n",
      "num = \n",
      "1104\n",
      "SENSOR 4 0.953865121238 -1.2816545328\n",
      "\n",
      "num = \n",
      "1105\n",
      "SENSOR 5 3.75050538274 -2.21025810438\n",
      "\n",
      "num = \n",
      "1106\n",
      "ODOMETRY 0.000643301288042 0.0998677942291 -0.202643646401\n",
      "\n",
      "sqP = \n",
      "[[1.4380480372686248, 0.0, 0.0], [-0.07583277086989382, 1.0501414805410714, 0.0], [0.037901768284231405, -0.1484164990266884, 0.38341409736265786]]\n",
      "sigma = \n",
      "[[ 0.1688518   9.50309188  1.74543803]\n",
      " [ 3.04494788  9.50309188  1.74543803]\n",
      " [ 0.01718626 11.60337484  1.74543803]\n",
      " [ 0.24465534  9.20625888  2.51226623]\n",
      " [-2.70724427  9.50309188  1.74543803]\n",
      " [ 0.32051734  7.40280892  1.74543803]\n",
      " [ 0.09304826  9.79992488  0.97860984]]\n",
      "sigmax = \n",
      "[[-0.02888561  9.60214694  1.76241369]\n",
      " [ 2.84721046  9.40305327  1.75504806]\n",
      " [-0.18055115 11.70385759  1.74725311]\n",
      " [ 0.04691792  9.3037439   2.53669467]\n",
      " [-2.90498169  9.4119617   1.70306625]\n",
      " [ 0.12277993  7.49816214  1.77718453]\n",
      " [-0.10468915  9.89998106  0.98803521]]\n",
      "sqP = \n",
      "[[1.5083664377958765, 0.0, 0.0], [-0.05872888891506596, 1.15383519890931, 0.0], [0.022494377418079047, -0.05466516489571548, 0.5889268579890129]]\n",
      "m = \n",
      "[-0.02230783  9.30685765  1.57603023]\n",
      "P = \n",
      "[[ 2.2747019  -0.08045748  0.03232396]\n",
      " [-0.08045748  1.1175583  -0.15191519]\n",
      " [ 0.03232396 -0.15191519  0.16926361]]\n",
      "num = \n",
      "1107\n",
      "SENSOR 4 0.937678468196 -1.17091065111\n",
      "\n",
      "num = \n",
      "1108\n",
      "SENSOR 5 3.86489395394 -1.98135942803\n",
      "\n",
      "num = \n",
      "1109\n",
      "ODOMETRY -0.000515670443917 0.100048418713 -0.201085070412\n",
      "\n",
      "sqP = \n",
      "[[1.508211489524309, 0.0, 0.0], [-0.05334628206114269, 1.0557994465366676, 0.0], [0.021431981109462518, -0.14280351467592176, 0.3852420543917331]]\n",
      "sigma = \n",
      "[[-0.02230783  9.30685765  1.57603023]\n",
      " [ 2.99411515  9.30685765  1.57603023]\n",
      " [-0.1290004  11.41845654  1.57603023]\n",
      " [ 0.02055613  9.02125062  2.34651434]\n",
      " [-3.03873081  9.30685765  1.57603023]\n",
      " [ 0.08438473  7.19525876  1.57603023]\n",
      " [-0.0651718   9.59246468  0.80554612]]\n",
      "sigmax = \n",
      "[[-0.22430818  9.40670201  1.57386681]\n",
      " [ 2.7921148   9.20806451  1.5906416 ]\n",
      " [-0.33100074 11.51750278  1.56324666]\n",
      " [-0.18144422  9.12109598  2.34863132]\n",
      " [ 3.04245415  9.20752434  1.56571185]\n",
      " [-0.11761561  7.29476576  1.58451156]\n",
      " [-0.26717214  9.69212463  0.79910627]]\n",
      "sqP = \n",
      "[[1.4344939326063613, 0.0, 0.0], [-0.12298525823871764, 1.1520093098809427, 0.0], [0.007481269671083263, -0.052215865378754056, 0.5893660633225093]]\n",
      "m = \n",
      "[0.58189404 9.15830969 1.45523943]\n",
      "P = \n",
      "[[ 2.05584604 -0.1580596   0.02107893]\n",
      " [-0.1580596   1.16715406 -0.16332518]\n",
      " [ 0.02107893 -0.16332518  0.1742597 ]]\n",
      "num = \n",
      "1110\n",
      "SENSOR 4 1.03088856231 -1.02236401255\n",
      "\n",
      "num = \n",
      "1111\n",
      "SENSOR 5 3.61390947737 -1.83253751296\n",
      "\n",
      "num = \n",
      "1112\n",
      "ODOMETRY -0.00070658966091 0.0999211581655 -0.201215641668\n",
      "\n",
      "sqP = \n",
      "[[1.4338221800437212, 0.0, 0.0], [-0.11023654249023682, 1.074710176378011, 0.0], [0.014701217446378055, -0.15046341830384152, 0.3891070951741806]]\n",
      "sigma = \n",
      "[[ 0.58189404  9.15830969  1.45523943]\n",
      " [ 3.4495384   9.15830969  1.45523943]\n",
      " [ 0.36142095 11.30773005  1.45523943]\n",
      " [ 0.61129647  8.85738286  2.23345362]\n",
      " [-2.28575032  9.15830969  1.45523943]\n",
      " [ 0.80236712  7.00888934  1.45523943]\n",
      " [ 0.5524916   9.45923653  0.67702524]]\n",
      "sigmax = \n",
      "[[ 0.38029329  9.24192081  1.51018366]\n",
      " [-3.03524765  9.0629521   1.42496376]\n",
      " [ 0.15982021 11.4013331   1.49056866]\n",
      " [ 0.40969573  8.93934257  2.29083211]\n",
      " [-2.48735107  9.09268073  1.37972436]\n",
      " [ 0.60076638  7.07846075  1.5271387 ]\n",
      " [ 0.35089086  9.54442676  0.7294877 ]]\n",
      "sqP = \n",
      "[[1.4428031616187547, 0.0, 0.0], [-0.016689388234035603, 1.1813632553307656, 0.0], [0.0474451839935328, -0.05496052574684869, 0.5911553560790306]]\n",
      "m = \n",
      "[-0.398932    9.2369611   1.56762358]\n",
      "P = \n",
      "[[ 2.08086259 -0.03137754  0.05678165]\n",
      " [-0.03137754  1.22369578 -0.18898033]\n",
      " [ 0.05678165 -0.18898033  0.18482393]]\n",
      "num = \n",
      "1113\n",
      "SENSOR 8 4.85461136115 -0.441193806736\n",
      "\n",
      "num = \n",
      "1114\n",
      "SENSOR 4 1.05741780526 -0.941454125061\n",
      "\n",
      "num = \n",
      "1115\n",
      "SENSOR 5 3.87262435523 -1.67878747955\n",
      "\n",
      "num = \n",
      "1116\n",
      "ODOMETRY -0.000453651906009 0.100058053226 -0.0495339656427\n",
      "\n",
      "sqP = \n",
      "[[1.4425195271444202, 0.0, 0.0], [-0.021751896815103113, 1.1059939598522768, 0.0], [0.039362831516403234, -0.1700950602869728, 0.39286405337966845]]\n",
      "sigma = \n",
      "[[-0.398932    9.2369611   1.56762358]\n",
      " [ 2.48610705  9.2369611   1.56762358]\n",
      " [-0.4424358  11.44894902  1.56762358]\n",
      " [-0.32020634  8.89677098  2.35335169]\n",
      " [-3.28397106  9.2369611   1.56762358]\n",
      " [-0.35542821  7.02497318  1.56762358]\n",
      " [-0.47765767  9.57715123  0.78189548]]\n",
      "sigmax = \n",
      "[[-0.60085423  9.32900864  1.52874571]\n",
      " [ 2.28418482  9.15779146  1.62858591]\n",
      " [-0.64435803 11.53921867  1.52477934]\n",
      " [-0.52212857  8.99159095  2.32183326]\n",
      " [ 2.79729202  9.13806106  1.58187207]\n",
      " [-0.55735044  7.11862443  1.53278565]\n",
      " [-0.6795799   9.66585614  0.735899  ]]\n",
      "sqP = \n",
      "[[1.4384117421840197, 0.0, 0.0], [-0.09586984558536306, 1.2028055151815906, 0.0], [0.04113820586586266, -0.056233170573947285, 0.5946262726234876]]\n",
      "m = \n",
      "[0.15972865 9.48211974 1.4019781 ]\n",
      "P = \n",
      "[[ 2.05979588 -0.034927    0.04254724]\n",
      " [-0.034927    0.2043679  -0.01939376]\n",
      " [ 0.04254724 -0.01939376  0.15620163]]\n",
      "num = \n",
      "1117\n",
      "SENSOR 8 4.91108468826 -0.314178952957\n",
      "\n",
      "num = \n",
      "1118\n",
      "SENSOR 4 0.889803683381 -0.862587924879\n",
      "\n",
      "num = \n",
      "1119\n",
      "SENSOR 5 4.001907275 -1.75368141539\n",
      "\n",
      "num = \n",
      "1120\n",
      "ODOMETRY -0.000374433876798 0.0999632649737 0.000227424570168\n",
      "\n",
      "sqP = \n",
      "[[1.4351989006242392, 0.0, 0.0], [-0.024335997231203037, 0.45141517910890283, 0.0], [0.029645536018143737, -0.04136392468166456, 0.39193341630254747]]\n",
      "sigma = \n",
      "[[ 0.15972865  9.48211974  1.4019781 ]\n",
      " [ 3.03012645  9.48211974  1.4019781 ]\n",
      " [ 0.11105666 10.3849501   1.4019781 ]\n",
      " [ 0.21901972  9.39939189  2.18584494]\n",
      " [-2.71066915  9.48211974  1.4019781 ]\n",
      " [ 0.20840064  8.57928938  1.4019781 ]\n",
      " [ 0.10043758  9.56484759  0.61811127]]\n",
      "sigmax = \n",
      "[[ 0.10974103  9.58091131  1.41784755]\n",
      " [ 2.98013883  9.3826877   1.41315322]\n",
      " [ 0.06106904 10.48439677  1.41302228]\n",
      " [ 0.1690321   9.4970695   2.20754053]\n",
      " [-2.76065677  9.39119001  1.3602241 ]\n",
      " [ 0.15841303  8.67719187  1.42263525]\n",
      " [ 0.05044996  9.66440593  0.62809881]]\n",
      "sqP = \n",
      "[[1.503750971045427, 0.0, 0.0], [-0.010165149082072097, 0.6425132949467444, 0.0], [0.020452382993820177, -0.02535750353160654, 0.5960001531845773]]\n",
      "m = \n",
      "[0.10473845 9.48546354 1.23930308]\n",
      "P = \n",
      "[[ 2.26089629 -0.00977915  0.02232629]\n",
      " [-0.00977915  0.14092724  0.01926993]\n",
      " [ 0.02232629  0.01926993  0.12255595]]\n",
      "num = \n",
      "1121\n",
      "SENSOR 8 4.89607963687 -0.383905377422\n",
      "\n",
      "num = \n",
      "1122\n",
      "SENSOR 4 1.03529311279 -1.08642084347\n",
      "\n",
      "num = \n",
      "1123\n",
      "SENSOR 5 3.6950450598 -1.62958875652\n",
      "\n",
      "num = \n",
      "1124\n",
      "ODOMETRY 0.000560387911335 0.100022258169 -0.000690144025918\n",
      "\n",
      "sqP = \n",
      "[[1.50362770836479, 0.0, 0.0], [-0.006503705763095248, 0.37534643162841497, 0.0], [0.014848282259816001, 0.05159632125328027, 0.3459382838383383]]\n",
      "sigma = \n",
      "[[ 0.10473845  9.48546354  1.23930308]\n",
      " [ 3.11199386  9.48546354  1.23930308]\n",
      " [ 0.09173104 10.23615641  1.23930308]\n",
      " [ 0.13443501  9.58865619  1.93117965]\n",
      " [-2.90251697  9.48546354  1.23930308]\n",
      " [ 0.11774586  8.73477068  1.23930308]\n",
      " [ 0.07504188  9.3822709   0.54742651]]\n",
      "sigmax = \n",
      "[[ 0.10459144  9.58488291  1.24971672]\n",
      " [ 3.11184685  9.38554518  1.24229885]\n",
      " [ 0.09158403 10.33570281  1.24842269]\n",
      " [ 0.134288    9.68772251  1.94454068]\n",
      " [-2.90266398  9.38833465  1.21566768]\n",
      " [ 0.11759885  8.83404619  1.25100899]\n",
      " [ 0.07489487  9.48195564  0.55488358]]\n",
      "sqP = \n",
      "[[1.5688081638737956, 0.0, 0.0], [-0.0017378438807595866, 0.5923890036518524, 0.0], [0.009672026606417338, 0.03107821335391858, 0.5654755573933427]]\n",
      "m = \n",
      "[0.10616727 9.54745408 1.33785366]\n",
      "P = \n",
      "[[ 2.46109384e+00 -1.92366186e-03  1.15328493e-02]\n",
      " [-1.92366186e-03  1.36493621e-01  2.84491299e-02]\n",
      " [ 1.15328493e-02  2.84491299e-02  1.11664251e-01]]\n",
      "num = \n",
      "1125\n",
      "SENSOR 8 4.77811944922 -0.344660362116\n",
      "\n",
      "num = \n",
      "1126\n",
      "SENSOR 4 0.6835522748 -1.12873248686\n",
      "\n",
      "num = \n",
      "1127\n",
      "SENSOR 5 3.73078201728 -1.73731680832\n",
      "\n",
      "num = \n",
      "1128\n",
      "ODOMETRY 0.000302602191039 0.0999189321218 -0.000444970990895\n",
      "\n",
      "sqP = \n",
      "[[1.5687873794144676, 0.0, 0.0], [-0.001226209415648713, 0.3694483959505338, 0.0], [0.007351441900550609, 0.07702873958351855, 0.3250796533492719]]\n",
      "sigma = \n",
      "[[ 0.10616727  9.54745408  1.33785366]\n",
      " [ 3.24374203  9.54745408  1.33785366]\n",
      " [ 0.10371485 10.28635087  1.33785366]\n",
      " [ 0.12087016  9.70151156  1.98801297]\n",
      " [-3.03140749  9.54745408  1.33785366]\n",
      " [ 0.10861969  8.80855729  1.33785366]\n",
      " [ 0.09146439  9.3933966   0.68769436]]\n",
      "sigmax = \n",
      "[[ 0.10603752  9.64690721  1.34850855]\n",
      " [-3.03957303  9.44795894  1.32759845]\n",
      " [ 0.1035851  10.38582984  1.34826462]\n",
      " [ 0.1207404   9.80079729  2.0001289 ]\n",
      " [-3.03153724  9.44804456  1.32679927]\n",
      " [ 0.10848993  8.90798399  1.34875242]\n",
      " [ 0.09133463  9.49299564  0.6968859 ]]\n",
      "sqP = \n",
      "[[1.4319952543082606, 0.0, 0.0], [0.08189658515892502, 0.5857763623989569, 0.0], [0.010437979078191155, 0.042620510423128075, 0.5516451595128732]]\n",
      "m = \n",
      "[-0.70024511  9.44390671  1.41020139]\n",
      "P = \n",
      "[[2.04645409 0.08760324 0.01397736]\n",
      " [0.08760324 0.13736168 0.03002803]\n",
      " [0.01397736 0.03002803 0.1143662 ]]\n",
      "num = \n",
      "1129\n",
      "SENSOR 8 4.69613208187 -0.387130423206\n",
      "\n",
      "num = \n",
      "1130\n",
      "SENSOR 4 0.787992521116 -1.29156833847\n",
      "\n",
      "num = \n",
      "1131\n",
      "SENSOR 5 3.83067456717 -1.73774870776\n",
      "\n",
      "num = \n",
      "1132\n",
      "ODOMETRY 0.00033245853007 0.099983722446 3.29125408185e-05\n",
      "\n",
      "sqP = \n",
      "[[1.4305432861504246, 0.0, 0.0], [0.061237743089929145, 0.3655292333016876, 0.0], [0.009770666435039096, 0.08051256086898198, 0.3283115323337387]]\n",
      "sigma = \n",
      "[[-0.70024511  9.44390671  1.41020139]\n",
      " [ 2.16084146  9.44390671  1.41020139]\n",
      " [-0.57776963 10.17496518  1.41020139]\n",
      " [-0.68070378  9.60493183  2.06682446]\n",
      " [-3.56133168  9.44390671  1.41020139]\n",
      " [-0.8227206   8.71284824  1.41020139]\n",
      " [-0.71978645  9.28288159  0.75357833]]\n",
      "sigmax = \n",
      "[[-0.70038748  9.52033262  1.34583624]\n",
      " [ 2.16069909  9.38828684  1.49320876]\n",
      " [-0.57791199 10.25868206  1.3556553 ]\n",
      " [-0.68084615  9.68260085  2.00396496]\n",
      " [ 2.72171125  9.35264889  1.45089296]\n",
      " [-0.82286297  8.78083821  1.33698147]\n",
      " [-0.71992881  9.35803521  0.6877321 ]]\n",
      "sqP = \n",
      "[[1.4401622365954494, 0.0, 0.0], [-0.0458241276672577, 0.587592604514262, 0.0], [0.05295704658357399, 0.04664566985800782, 0.553544842931145]]\n",
      "m = \n",
      "[0.09194886 9.36167682 1.35304679]\n",
      "P = \n",
      "[[ 2.07077241 -0.04956324  0.05657331]\n",
      " [-0.04956324  0.13465372  0.02344041]\n",
      " [ 0.05657331  0.02344041  0.11759788]]\n",
      "num = \n",
      "1133\n",
      "SENSOR 8 4.43987229987 -0.336598652401\n",
      "\n",
      "num = \n",
      "1134\n",
      "SENSOR 4 0.824851032983 -1.37173579521\n",
      "\n",
      "num = \n",
      "1135\n",
      "SENSOR 5 3.9011475333 -1.83111676621\n",
      "\n",
      "num = \n",
      "1136\n",
      "ODOMETRY 0.0003202499917 0.0998020171825 -0.000105496843961\n",
      "\n",
      "sqP = \n",
      "[[1.4390178639600963, 0.0, 0.0], [-0.03444240941342898, 0.36533195426514403, 0.0], [0.039313834316155996, 0.06786832679910478, 0.33383557015469795]]\n",
      "sigma = \n",
      "[[ 0.09194886  9.36167682  1.35304679]\n",
      " [ 2.96998459  9.36167682  1.35304679]\n",
      " [ 0.02306405 10.09234073  1.35304679]\n",
      " [ 0.17057653  9.49741348  2.02071793]\n",
      " [-2.78608686  9.36167682  1.35304679]\n",
      " [ 0.16083368  8.63101292  1.35304679]\n",
      " [ 0.0133212   9.22594017  0.68537565]]\n",
      "sigmax = \n",
      "[[ 0.09231424  9.46123513  1.36226033]\n",
      " [ 2.97034996  9.26315605  1.37008796]\n",
      " [ 0.02342942 10.19229709  1.35538585]\n",
      " [ 0.1709419   9.59594049  2.03772298]\n",
      " [-2.78572149  9.2679566   1.31821484]\n",
      " [ 0.16119905  8.72970093  1.36909112]\n",
      " [ 0.01368657  9.32591457  0.68674075]]\n",
      "sqP = \n",
      "[[1.507814401685804, 0.0, 0.0], [-0.007737634266784207, 0.5876768209839255, 0.0], [0.021261018587113722, 0.03804695634703567, 0.5589398439980878]]\n",
      "m = \n",
      "[0.0940951  9.17807034 1.32429167]\n",
      "P = \n",
      "[[ 2.27316299 -0.00885822  0.02424196]\n",
      " [-0.00885822  0.13473072  0.02308368]\n",
      " [ 0.02424196  0.02308368  0.11387333]]\n",
      "num = \n",
      "1137\n",
      "SENSOR 8 4.2295439355 -0.31030833621\n",
      "\n",
      "num = \n",
      "1138\n",
      "SENSOR 4 0.562121580647 -1.48855015673\n",
      "\n",
      "num = \n",
      "1139\n",
      "SENSOR 5 4.05047851622 -1.85523162918\n",
      "\n",
      "num = \n",
      "1140\n",
      "ODOMETRY -0.00100295830048 0.100070287738 -0.000357513844835\n",
      "\n",
      "sqP = \n",
      "[[1.5077012269698291, 0.0, 0.0], [-0.005875315890041808, 0.3670098035015018, 0.0], [0.01607875831205856, 0.06315403473831915, 0.33109874118744403]]\n",
      "sigma = \n",
      "[[ 0.0940951   9.17807034  1.32429167]\n",
      " [ 3.10949755  9.17807034  1.32429167]\n",
      " [ 0.08234447  9.91208995  1.32429167]\n",
      " [ 0.12625261  9.30437841  1.98648915]\n",
      " [-2.92130736  9.17807034  1.32429167]\n",
      " [ 0.10584573  8.44405073  1.32429167]\n",
      " [ 0.06193758  9.05176227  0.66209419]]\n",
      "sigmax = \n",
      "[[ 0.09430985  9.27742786  1.33370052]\n",
      " [ 3.10971231  9.0783187   1.32746233]\n",
      " [ 0.08255922 10.01155116  1.33253238]\n",
      " [ 0.12646737  9.40338205  1.99908768]\n",
      " [-2.9210926   9.08068701  1.30245294]\n",
      " [ 0.10606048  8.54329084  1.33486735]\n",
      " [ 0.06215233  9.15137093  0.66830363]]\n",
      "sqP = \n",
      "[[1.5727224916087368, 0.0, 0.0], [-0.0012947570492943573, 0.5882589064198209, 0.0], [0.009397361000720273, 0.036464209913447065, 0.5562139647832143]]\n",
      "m = \n",
      "[0.09462563 8.94314451 1.31075932]\n",
      "P = \n",
      "[[ 2.47339699e+00 -1.62957076e-03  1.13666837e-02]\n",
      " [-1.62957076e-03  1.36847001e-01  1.82847818e-02]\n",
      " [ 1.13666837e-02  1.82847818e-02  1.10099759e-01]]\n",
      "num = \n",
      "1141\n",
      "SENSOR 8 4.14141953883 -0.401434359312\n",
      "\n",
      "num = \n",
      "1142\n",
      "SENSOR 4 0.698270524653 -1.66266898697\n",
      "\n",
      "num = \n",
      "1143\n",
      "SENSOR 5 4.02646711957 -1.85655804032\n",
      "\n",
      "num = \n",
      "1144\n",
      "ODOMETRY -0.000188063237756 0.0999460648438 -0.000367434508236\n",
      "\n",
      "sqP = \n",
      "[[1.5727037192907543, 0.0, 0.0], [-0.0010361587776337232, 0.3699269217958858, 0.0], [0.007227479370978929, 0.04944833571817185, 0.3280280234380182]]\n",
      "sigma = \n",
      "[[ 0.09462563  8.94314451  1.31075932]\n",
      " [ 3.24003307  8.94314451  1.31075932]\n",
      " [ 0.09255331  9.68299836  1.31075932]\n",
      " [ 0.10908059  9.04204118  1.96681537]\n",
      " [-3.05078181  8.94314451  1.31075932]\n",
      " [ 0.09669794  8.20329067  1.31075932]\n",
      " [ 0.08017067  8.84424784  0.65470327]]\n",
      "sigmax = \n",
      "[[ 0.09326515  9.04277655  1.32011449]\n",
      " [-3.04451271  8.84354889  1.30102415]\n",
      " [ 0.09119284  9.78264957  1.319908  ]\n",
      " [ 0.10772011  9.14152759  1.97760968]\n",
      " [-3.05214228  8.84347751  1.3017843 ]\n",
      " [ 0.09533747  8.30290311  1.32032094]\n",
      " [ 0.0788102   8.9440047   0.66261734]]\n",
      "sqP = \n",
      "[[1.4319943342881838, 0.0, 0.0], [0.08194678923217015, 0.5831130833641832, 0.0], [0.009355361565408393, 0.0276584581951182, 0.5543360666990543]]\n",
      "m = \n",
      "[-0.715264    8.82799392  1.3207032 ]\n",
      "P = \n",
      "[[2.04652863 0.08852075 0.01055339]\n",
      " [0.08852075 0.14204175 0.01079042]\n",
      " [0.01055339 0.01079042 0.10686095]]\n",
      "num = \n",
      "1145\n",
      "SENSOR 8 4.49059154755 -0.359642792629\n",
      "\n",
      "num = \n",
      "1146\n",
      "SENSOR 4 0.829352455695 -1.82738715515\n",
      "\n",
      "num = \n",
      "1147\n",
      "SENSOR 5 3.9198043194 -1.85847185083\n",
      "\n",
      "num = \n",
      "1148\n",
      "ODOMETRY 0.000145108773369 0.0999890308492 -0.0008287350519\n",
      "\n",
      "sqP = \n",
      "[[1.430569337182372, 0.0, 0.0], [0.06187798533969722, 0.37176991431375667, 0.0], [0.007377052627880644, 0.027796613672998422, 0.32562843580552203]]\n",
      "sigma = \n",
      "[[-0.715264    8.82799392  1.3207032 ]\n",
      " [ 2.14587467  8.82799392  1.3207032 ]\n",
      " [-0.59150803  9.57153375  1.3207032 ]\n",
      " [-0.7005099   8.88358715  1.97196007]\n",
      " [-3.57640268  8.82799392  1.3207032 ]\n",
      " [-0.83901998  8.08445409  1.3207032 ]\n",
      " [-0.73001811  8.77240069  0.66944633]]\n",
      "sigmax = \n",
      "[[-0.7158195   8.90343289  1.25514271]\n",
      " [ 2.14531917  8.77364898  1.40458313]\n",
      " [-0.59206353  9.65448857  1.26495633]\n",
      " [-0.7010654   8.95998516  1.90751971]\n",
      " [ 2.70622713  8.73735575  1.36282134]\n",
      " [-0.83957547  8.1512233   1.24633191]\n",
      " [-0.73057361  8.8468642   0.60277998]]\n",
      "sqP = \n",
      "[[1.4401728159478708, 0.0, 0.0], [-0.04501703939638837, 0.586673550106486, 0.0], [0.05336742917049941, 0.01690053851860894, 0.5536103073422325]]\n",
      "m = \n",
      "[0.05808461 9.01118188 1.28358983]\n",
      "P = \n",
      "[[ 2.07096654 -0.04923824  0.05726547]\n",
      " [-0.04923824  0.13707674  0.00292024]\n",
      " [ 0.05726547  0.00292024  0.10762338]]\n",
      "num = \n",
      "1149\n",
      "SENSOR 8 4.0305518916 -0.410490083997\n",
      "\n",
      "num = \n",
      "1150\n",
      "SENSOR 4 0.73104561841 -1.91623346334\n",
      "\n",
      "num = \n",
      "1151\n",
      "SENSOR 5 3.95455389546 -1.81930736734\n",
      "\n",
      "num = \n",
      "1152\n",
      "ODOMETRY 0.000317286436698 0.0999382739112 -0.000139268224043\n",
      "\n",
      "sqP = \n",
      "[[1.4390853132404413, 0.0, 0.0], [-0.03421495304447135, 0.3686544097768904, 0.0], [0.03979296440142431, 0.011614560310791971, 0.32543049072273705]]\n",
      "sigma = \n",
      "[[ 0.05808461  9.01118188  1.28358983]\n",
      " [ 2.93625523  9.01118188  1.28358983]\n",
      " [-0.0103453   9.74849069  1.28358983]\n",
      " [ 0.13767054  9.034411    1.93445081]\n",
      " [-2.82008602  9.01118188  1.28358983]\n",
      " [ 0.12651451  8.27387306  1.28358983]\n",
      " [-0.02150132  8.98795275  0.63272885]]\n",
      "sigmax = \n",
      "[[ 0.05740098  9.11100144  1.28940887]\n",
      " [ 2.93557161  8.91329043  1.30396314]\n",
      " [-0.01102893  9.84847452  1.28256994]\n",
      " [ 0.13698691  9.13345198  1.94818728]\n",
      " [-2.82076965  8.91632083  1.25197989]\n",
      " [ 0.12583089  8.37306111  1.29622056]\n",
      " [-0.02218495  9.08791898  0.63059362]]\n",
      "sqP = \n",
      "[[1.5078861634128844, 0.0, 0.0], [-0.008792159941803187, 0.5859262638745432, 0.0], [0.021173073077999587, 0.005268791737909371, 0.5551867035257667]]\n",
      "m = \n",
      "[0.06412895 8.79658051 1.35832894]\n",
      "P = \n",
      "[[ 2.27336787 -0.00989204  0.02402922]\n",
      " [-0.00989204  0.13744015  0.01008982]\n",
      " [ 0.02402922  0.01008982  0.10521708]]\n",
      "num = \n",
      "1153\n",
      "SENSOR 8 3.94074309758 -0.449033720052\n",
      "\n",
      "num = \n",
      "1154\n",
      "SENSOR 4 0.964327453377 -2.01100457015\n",
      "\n",
      "num = \n",
      "1155\n",
      "SENSOR 5 4.12377150794 -1.90702070002\n",
      "\n",
      "num = \n",
      "1156\n",
      "ODOMETRY -0.000489035127382 0.10010816554 -0.000185002568801\n",
      "\n",
      "sqP = \n",
      "[[1.5077691715652148, 0.0, 0.0], [-0.006560714558304798, 0.37067115528165123, 0.0], [0.01593693354468917, 0.02750249373236886, 0.32281063891674683]]\n",
      "sigma = \n",
      "[[ 0.06412895  8.79658051  1.35832894]\n",
      " [ 3.07966729  8.79658051  1.35832894]\n",
      " [ 0.05100752  9.53792282  1.35832894]\n",
      " [ 0.09600281  8.8515855   2.00395022]\n",
      " [-2.9514094   8.79658051  1.35832894]\n",
      " [ 0.07725038  8.0552382   1.35832894]\n",
      " [ 0.03225508  8.74157552  0.71270766]]\n",
      "sigmax = \n",
      "[[ 0.06430696  8.89631132  1.36476513]\n",
      " [ 3.07984531  8.69683184  1.36448205]\n",
      " [ 0.05118554  9.63772949  1.363456  ]\n",
      " [ 0.09618083  8.95106054  2.0135614 ]\n",
      " [-2.95123138  8.69845016  1.33940559]\n",
      " [ 0.07742839  8.15487597  1.36607315]\n",
      " [ 0.0324331   8.84146079  0.71596231]]\n",
      "sqP = \n",
      "[[1.5727888935101169, 0.0, 0.0], [-0.0016566042121746196, 0.5878568880198531, 0.0], [0.009299792217979006, 0.0155418068461118, 0.5522478836702495]]\n",
      "m = \n",
      "[0.0635157  8.62573669 1.2780003 ]\n",
      "P = \n",
      "[[ 2.47360749e+00 -2.09331225e-03  1.13187423e-02]\n",
      " [-2.09331225e-03  1.33535107e-01  4.07519173e-03]\n",
      " [ 1.13187423e-02  4.07519173e-03  1.08955594e-01]]\n",
      "num = \n",
      "1157\n",
      "SENSOR 8 3.91534330666 -0.437399649948\n",
      "\n",
      "num = \n",
      "1158\n",
      "SENSOR 4 0.962844439912 -2.09903655675\n",
      "\n",
      "num = \n",
      "1159\n",
      "SENSOR 5 4.0517244807 -1.90944618344\n",
      "\n",
      "num = \n",
      "1160\n",
      "ODOMETRY -0.000448212051337 0.100133168601 -0.0012892610486\n",
      "\n",
      "sqP = \n",
      "[[1.5727706424518066, 0.0, 0.0], [-0.0013309710853611578, 0.36542213291978837, 0.0], [0.007196689729730538, 0.011178223606480563, 0.3298163865768537]]\n",
      "sigma = \n",
      "[[ 0.0635157   8.62573669  1.2780003 ]\n",
      " [ 3.20905699  8.62573669  1.2780003 ]\n",
      " [ 0.06085376  9.35658096  1.2780003 ]\n",
      " [ 0.07790908  8.64809314  1.93763307]\n",
      " [-3.08202558  8.62573669  1.2780003 ]\n",
      " [ 0.06617765  7.89489243  1.2780003 ]\n",
      " [ 0.04912233  8.60338025  0.61836752]]\n",
      "sigmax = \n",
      "[[ 0.06284167  8.72564609  1.28430561]\n",
      " [-3.07480236  8.52585297  1.27130054]\n",
      " [ 0.06017972  9.45650679  1.28403963]\n",
      " [ 0.07723505  8.74790144  1.94537571]\n",
      " [-3.08269962  8.52580318  1.27208954]\n",
      " [ 0.06550361  7.99478469  1.28457154]\n",
      " [ 0.04844829  8.70337005  0.6232342 ]]\n",
      "sqP = \n",
      "[[1.4319945135906296, 0.0, 0.0], [0.08190966685798592, 0.5783081409198746, 0.0], [0.006848247135104405, 0.006125999592222993, 0.5560567265862298]]\n",
      "m = \n",
      "[-0.73778681  8.56629083  1.3057428 ]\n",
      "P = \n",
      "[[ 2.04645675e+00  8.82055221e-02  6.87627026e-03]\n",
      " [ 8.82055221e-02  1.36172189e-01 -1.13635388e-03]\n",
      " [ 6.87627026e-03 -1.13635388e-03  1.05830701e-01]]\n",
      "num = \n",
      "1161\n",
      "SENSOR 8 3.8900635959 -0.450094707651\n",
      "\n",
      "num = \n",
      "1162\n",
      "SENSOR 4 1.15499811144 -2.14063500493\n",
      "\n",
      "num = \n",
      "1163\n",
      "SENSOR 5 4.15361685125 -1.97034676565\n",
      "\n",
      "num = \n",
      "1164\n",
      "ODOMETRY -0.000333404857971 0.0999912335863 0.000192681272112\n",
      "\n",
      "sqP = \n",
      "[[1.4305442131368598, 0.0, 0.0], [0.06165871788111413, 0.3638274199982172, 0.0], [0.004806751303494381, -0.0039379439942130124, 0.3252569584560149]]\n",
      "sigma = \n",
      "[[-0.73778681  8.56629083  1.3057428 ]\n",
      " [ 2.12330162  8.56629083  1.3057428 ]\n",
      " [-0.61446937  9.29394567  1.3057428 ]\n",
      " [-0.72817331  8.55841494  1.95625672]\n",
      " [-3.59887523  8.56629083  1.3057428 ]\n",
      " [-0.86110424  7.83863599  1.3057428 ]\n",
      " [-0.74740031  8.57416671  0.65522888]]\n",
      "sigmax = \n",
      "[[-0.73952428  8.64035508  1.23835483]\n",
      " [ 2.12156415  8.513777    1.39100094]\n",
      " [-0.61620685  9.37573654  1.24797686]\n",
      " [-0.72991078  8.63312359  1.88958387]\n",
      " [ 2.6825726   8.47646561  1.34999301]\n",
      " [-0.86284172  7.90384873  1.22975629]\n",
      " [-0.74913778  8.64757972  0.58713202]]\n",
      "sqP = \n",
      "[[1.4401549857354374, 0.0, 0.0], [-0.044492148480642285, 0.5809204523487874, 0.0], [0.05426165874853633, -0.000998205968514185, 0.553597248500443]]\n",
      "m = \n",
      "[0.05069181 8.48243635 1.22569437]\n",
      "P = \n",
      "[[ 2.07095069 -0.04839883  0.05878415]\n",
      " [-0.04839883  0.13011576 -0.00662927]\n",
      " [ 0.05878415 -0.00662927  0.10876357]]\n",
      "num = \n",
      "1165\n",
      "SENSOR 8 3.77038644114 -0.564054467112\n",
      "\n",
      "num = \n",
      "1166\n",
      "SENSOR 4 0.97476218836 -2.24080497152\n",
      "\n",
      "num = \n",
      "1167\n",
      "SENSOR 5 4.17413980453 -1.93610976395\n",
      "\n",
      "num = \n",
      "1168\n",
      "ODOMETRY 0.000116797154257 0.100137172993 -0.000390404927584\n",
      "\n",
      "sqP = \n",
      "[[1.4390798054170826, 0.0, 0.0], [-0.033631787921767214, 0.35914434590514355, 0.0], [0.04084843016184649, -0.014633303700393032, 0.3269263522524363]]\n",
      "sigma = \n",
      "[[ 0.05069181  8.48243635  1.22569437]\n",
      " [ 2.92885143  8.48243635  1.22569437]\n",
      " [-0.01657176  9.20072505  1.22569437]\n",
      " [ 0.13238867  8.45316975  1.87954707]\n",
      " [-2.8274678   8.48243635  1.22569437]\n",
      " [ 0.11795539  7.76414766  1.22569437]\n",
      " [-0.03100505  8.51170296  0.57184166]]\n",
      "sigmax = \n",
      "[[ 0.05055109  8.58230083  1.23072764]\n",
      " [ 2.9287107   8.38470639  1.24683911]\n",
      " [-0.01671249  9.30070199  1.22400408]\n",
      " [ 0.13224795  8.55229039  1.89271309]\n",
      " [-2.82760852  8.38732768  1.19483035]\n",
      " [ 0.11781467  7.86344801  1.23742843]\n",
      " [-0.03114577  8.6116451   0.5687086 ]]\n",
      "sqP = \n",
      "[[1.507896010262236, 0.0, 0.0], [-0.00904144239363754, 0.5800476173578001, 0.0], [0.021450375204744482, -0.009600391606971636, 0.5560648282922057]]\n",
      "m = \n",
      "[0.0542429  8.41354987 1.27598075]\n",
      "P = \n",
      "[[ 2.27340116 -0.01040622  0.02454171]\n",
      " [-0.01040622  0.12671945 -0.00688992]\n",
      " [ 0.02454171 -0.00688992  0.10559497]]\n",
      "num = \n",
      "1169\n",
      "SENSOR 8 3.63226086454 -0.540987639383\n",
      "\n",
      "num = \n",
      "1170\n",
      "SENSOR 4 1.10417906905 -2.38354517123\n",
      "\n",
      "num = \n",
      "1171\n",
      "SENSOR 5 4.30329593999 -2.0340943866\n",
      "\n",
      "num = \n",
      "1172\n",
      "SENSOR 7 4.94033277705 -0.757833609985\n",
      "\n",
      "num = \n",
      "1173\n",
      "ODOMETRY -0.000590074857926 0.0999133906589 -0.000728548241666\n",
      "\n",
      "sqP = \n",
      "[[1.5077802094408401, 0.0, 0.0], [-0.006901685155739515, 0.3559098392588902, 0.0], [0.016276713262139367, -0.019042971773896935, 0.3239867409587385]]\n",
      "sigma = \n",
      "[[ 0.0542429   8.41354987  1.27598075]\n",
      " [ 3.06980332  8.41354987  1.27598075]\n",
      " [ 0.04043953  9.12536955  1.27598075]\n",
      " [ 0.08679633  8.37546392  1.92395424]\n",
      " [-2.96131752  8.41354987  1.27598075]\n",
      " [ 0.06804627  7.70173019  1.27598075]\n",
      " [ 0.02168947  8.45163581  0.62800727]]\n",
      "sigmax = \n",
      "[[ 0.05396929  8.51353913  1.2814215 ]\n",
      " [ 3.06952971  8.31366978  1.2831517 ]\n",
      " [ 0.04016592  9.22542438  1.28004084]\n",
      " [ 0.08652272  8.47522312  1.93264652]\n",
      " [-2.96159113  8.31503758  1.25801463]\n",
      " [ 0.06777266  7.80163482  1.28280113]\n",
      " [ 0.02141586  8.55174918  0.63019072]]\n",
      "sqP = \n",
      "[[1.5728044139797135, 0.0, 0.0], [-0.0020877465603195114, 0.5783578420685084, 0.0], [0.009397215075793308, -0.01046575138759629, 0.553071960949022]]\n",
      "m = \n",
      "[0.05444328 8.25096427 1.26133583]\n",
      "P = \n",
      "[[ 2.47364555e+00 -2.08456498e-03  1.10072160e-02]\n",
      " [-2.08456498e-03  9.75877038e-02  1.42459065e-02]\n",
      " [ 1.10072160e-02  1.42459065e-02  8.74786582e-02]]\n",
      "num = \n",
      "1174\n",
      "SENSOR 8 3.48279289223 -0.442671033673\n",
      "\n",
      "num = \n",
      "1175\n",
      "SENSOR 4 1.21367576112 -2.38923397243\n",
      "\n",
      "num = \n",
      "1176\n",
      "SENSOR 5 4.11351339144 -2.12124027897\n",
      "\n",
      "num = \n",
      "1177\n",
      "SENSOR 7 4.9471083243 -0.863368530901\n",
      "\n",
      "num = \n",
      "1178\n",
      "ODOMETRY 0.000666027634388 0.0999961749585 -0.00041098921494\n",
      "\n",
      "sqP = \n",
      "[[1.5727827412514908, 0.0, 0.0], [-0.0013253991954830578, 0.31238749516917386, 0.0], [0.006998561058861059, 0.045633012246570896, 0.29214261328170066]]\n",
      "sigma = \n",
      "[[ 0.05444328  8.25096427  1.26133583]\n",
      " [ 3.20000877  8.25096427  1.26133583]\n",
      " [ 0.05179249  8.87573926  1.26133583]\n",
      " [ 0.06844041  8.34223029  1.84562106]\n",
      " [-3.0911222   8.25096427  1.26133583]\n",
      " [ 0.05709408  7.62618928  1.26133583]\n",
      " [ 0.04044616  8.15969824  0.67705061]]\n",
      "sigmax = \n",
      "[[ 0.05312466  8.35073281  1.26671389]\n",
      " [-3.08449516  8.15121788  1.25556146]\n",
      " [ 0.05047386  8.97552171  1.2664494 ]\n",
      " [ 0.06712178  8.44191379  1.85239501]\n",
      " [-3.09244082  8.15117515  1.25635418]\n",
      " [ 0.05577546  7.72594321  1.26697834]\n",
      " [ 0.03912754  8.25953229  0.68103171]]\n",
      "sqP = \n",
      "[[1.4319935431371782, 0.0, 0.0], [0.08201131222901716, 0.548095332780866, 0.0], [0.005855464405011583, 0.024143730116427606, 0.5340040188064807]]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "m = \n",
      "[-0.75061342  8.17659575  1.32393752]\n",
      "P = \n",
      "[[2.04588082 0.08589409 0.00730451]\n",
      " [0.08589409 0.09635337 0.01216672]\n",
      " [0.00730451 0.01216672 0.0836204 ]]\n",
      "num = \n",
      "1179\n",
      "SENSOR 8 3.31329949365 -0.442208158787\n",
      "\n",
      "num = \n",
      "1180\n",
      "SENSOR 4 1.14087202904 -2.42970915883\n",
      "\n",
      "num = \n",
      "1181\n",
      "SENSOR 5 4.28986062879 -2.13951007973\n",
      "\n",
      "num = \n",
      "1182\n",
      "SENSOR 7 4.50725975692 -0.916450378366\n",
      "\n",
      "num = \n",
      "1183\n",
      "ODOMETRY 0.000123003750952 0.0998768096926 -0.000338091992771\n",
      "\n",
      "sqP = \n",
      "[[1.4303429013645672, 0.0, 0.0], [0.0600513973046328, 0.30454424702753335, 0.0], [0.005106821304696513, 0.03894361144839158, 0.2864920803908792]]\n",
      "sigma = \n",
      "[[-0.75061342  8.17659575  1.32393752]\n",
      " [ 2.11007239  8.17659575  1.32393752]\n",
      " [-0.63051062  8.78568425  1.32393752]\n",
      " [-0.74039977  8.25448298  1.89692168]\n",
      " [-3.61129922  8.17659575  1.32393752]\n",
      " [-0.87071621  7.56750726  1.32393752]\n",
      " [-0.76082706  8.09870853  0.75095336]]\n",
      "sigmax = \n",
      "[[-0.75035838  8.24976543  1.2557801 ]\n",
      " [ 2.11032742  8.12518909  1.40970809]\n",
      " [-0.63025558  8.86649306  1.26503786]\n",
      " [-0.74014474  8.32834496  1.82951514]\n",
      " [ 2.67214113  8.08739893  1.36913886]\n",
      " [-0.87046117  7.63198361  1.24750432]\n",
      " [-0.76057202  8.17117827  0.68205218]]\n",
      "sqP = \n",
      "[[1.4401077181452242, 0.0, 0.0], [-0.04652442198550502, 0.5464150971137306, 0.0], [0.05480771887347701, 0.021382424785526294, 0.5313130666846345]]\n",
      "m = \n",
      "[0.07022407 7.96426977 1.42011607]\n",
      "P = \n",
      "[[ 2.07013973 -0.04910149  0.05785105]\n",
      " [-0.04910149  0.08883301  0.00778665]\n",
      " [ 0.05785105  0.00778665  0.08691521]]\n",
      "num = \n",
      "1184\n",
      "SENSOR 8 3.58031102591 -0.631152189894\n",
      "\n",
      "num = \n",
      "1185\n",
      "SENSOR 4 1.28631748205 -2.52280355497\n",
      "\n",
      "num = \n",
      "1186\n",
      "SENSOR 5 4.29694827231 -2.13689954352\n",
      "\n",
      "num = \n",
      "1187\n",
      "SENSOR 7 4.75408087282 -0.865684520747\n",
      "\n",
      "num = \n",
      "1188\n",
      "ODOMETRY -0.000346457382135 0.100082898443 0.000286890929699\n",
      "\n",
      "sqP = \n",
      "[[1.438798015578543, 0.0, 0.0], [-0.03412674035686128, 0.2960884575811738, 0.0], [0.04020789966843569, 0.03093269690155593, 0.2904164374443747]]\n",
      "sigma = \n",
      "[[ 7.02240650e-02  7.96426977e+00  1.42011607e+00]\n",
      " [ 2.94782010e+00  7.96426977e+00  1.42011607e+00]\n",
      " [ 1.97058431e-03  8.55644669e+00  1.42011607e+00]\n",
      " [ 1.50639864e-01  8.02613517e+00  2.00094895e+00]\n",
      " [-2.80737197e+00  7.96426977e+00  1.42011607e+00]\n",
      " [ 1.38477546e-01  7.37209286e+00  1.42011607e+00]\n",
      " [-1.01917343e-02  7.90240438e+00  8.39283199e-01]]\n",
      "sigmax = \n",
      "[[ 7.00089768e-02  8.06389955e+00  1.42713632e+00]\n",
      " [ 2.94760501e+00  7.86625982e+00  1.43933652e+00]\n",
      " [ 1.75549607e-03  8.65632328e+00  1.42032517e+00]\n",
      " [ 1.50424776e-01  8.12487905e+00  2.01594969e+00]\n",
      " [-2.80758705e+00  7.86992355e+00  1.38734157e+00]\n",
      " [ 1.38262457e-01  7.47101188e+00  1.43391478e+00]\n",
      " [-1.04068226e-02  8.00227613e+00  8.38277583e-01]]\n",
      "sqP = \n",
      "[[1.5076209865997106, 0.0, 0.0], [-0.006764396857048093, 0.543948610671965, 0.0], [0.0203343077006668, 0.015899711191663255, 0.5350063780672355]]\n",
      "m = \n",
      "[0.06595768 8.03267462 1.32390104]\n",
      "P = \n",
      "[[ 2.27259585 -0.00750041  0.02305714]\n",
      " [-0.00750041  0.07843695  0.00833144]\n",
      " [ 0.02305714  0.00833144  0.08882409]]\n",
      "num = \n",
      "1189\n",
      "SENSOR 8 3.24455116705 -0.576310948619\n",
      "\n",
      "num = \n",
      "1190\n",
      "SENSOR 4 1.27283382282 -2.55880342549\n",
      "\n",
      "num = \n",
      "1191\n",
      "SENSOR 5 4.41483458904 -2.05309918925\n",
      "\n",
      "num = \n",
      "1192\n",
      "SENSOR 7 4.63121738461 -0.874521274014\n",
      "\n",
      "num = \n",
      "1193\n",
      "ODOMETRY -0.000955326333333 0.100096022413 -0.000648521461897\n",
      "\n",
      "sqP = \n",
      "[[1.5075131348466893, 0.0, 0.0], [-0.0049753515041593924, 0.280021770203855, 0.0], [0.01529481992728247, 0.030024590727104904, 0.29612274317430404]]\n",
      "sigma = \n",
      "[[ 0.06595768  8.03267462  1.32390104]\n",
      " [ 3.08098395  8.03267462  1.32390104]\n",
      " [ 0.05600697  8.59271816  1.32390104]\n",
      " [ 0.09654732  8.0927238   1.91614653]\n",
      " [-2.94906859  8.03267462  1.32390104]\n",
      " [ 0.07590838  7.47263108  1.32390104]\n",
      " [ 0.03536804  7.97262543  0.73165556]]\n",
      "sigmax = \n",
      "[[ 0.06589811  8.13254217  1.33046289]\n",
      " [ 3.08092438  7.93277759  1.32999784]\n",
      " [ 0.05594741  8.69264606  1.32946883]\n",
      " [ 0.09648775  8.19234394  1.92575975]\n",
      " [-2.94912816  7.93443418  1.30478552]\n",
      " [ 0.07584881  7.57242839  1.3314563 ]\n",
      " [ 0.03530847  8.07264696  0.7351599 ]]\n",
      "sqP = \n",
      "[[1.5725312516368284, 0.0, 0.0], [-0.0009920409601550464, 0.5354850825547068, 0.0], [0.008939057630400217, 0.017295601619966926, 0.536925004626109]]\n",
      "m = \n",
      "[0.0664748  7.88641976 1.33993558]\n",
      "P = \n",
      "[[ 2.47279718e+00 -1.19518349e-03  1.06642884e-02]\n",
      " [-1.19518349e-03  8.13599916e-02  8.31509096e-03]\n",
      " [ 1.06642884e-02  8.31509096e-03  8.55111243e-02]]\n",
      "num = \n",
      "1194\n",
      "SENSOR 8 3.24825762533 -0.599421325277\n",
      "\n",
      "num = \n",
      "1195\n",
      "SENSOR 4 1.50961115908 -2.52257610457\n",
      "\n",
      "num = \n",
      "1196\n",
      "SENSOR 5 4.51886453403 -2.10172063614\n",
      "\n",
      "num = \n",
      "1197\n",
      "SENSOR 7 4.67172096933 -0.894687907489\n",
      "\n",
      "num = \n",
      "1198\n",
      "ODOMETRY 0.000425896177498 0.100141662167 0.000472712559654\n",
      "\n",
      "sqP = \n",
      "[[1.5725130129665355, 0.0, 0.0], [-0.0007600468069599362, 0.2852357164235791, 0.0], [0.006781685314412941, 0.02916971782901836, 0.29088530496002857]]\n",
      "sigma = \n",
      "[[ 0.0664748   7.88641976  1.33993558]\n",
      " [ 3.21150083  7.88641976  1.33993558]\n",
      " [ 0.06495471  8.4568912   1.33993558]\n",
      " [ 0.08003817  7.9447592   1.92170619]\n",
      " [-3.07855122  7.88641976  1.33993558]\n",
      " [ 0.0679949   7.31594833  1.33993558]\n",
      " [ 0.05291143  7.82808033  0.75816497]]\n",
      "sigmax = \n",
      "[[ 0.06487095  7.98630102  1.34648912]\n",
      " [-3.07328833  7.7865616   1.33303914]\n",
      " [ 0.06335086  8.5567823   1.34633729]\n",
      " [ 0.07843433  8.04454238  1.92961382]\n",
      " [-3.08015507  7.7865166   1.333725  ]\n",
      " [ 0.06639105  7.41581951  1.34664094]\n",
      " [ 0.05130758  7.92804128  0.76336323]]\n",
      "sqP = \n",
      "[[1.4319917392019128, 0.0, 0.0], [0.08215717591499491, 0.5319235451194527, 0.0], [0.0067721224837730505, 0.015800621003025792, 0.5336154999786148]]\n",
      "m = \n",
      "[-0.74646592  7.77306173  1.25487577]\n",
      "P = \n",
      "[[2.04568586 0.0859263  0.00786089]\n",
      " [0.0859263  0.08438252 0.00756138]\n",
      " [0.00786089 0.00756138 0.08540806]]\n",
      "num = \n",
      "1199\n",
      "SENSOR 8 3.23972718873 -0.58466657353\n",
      "\n",
      "num = \n",
      "1200\n",
      "SENSOR 4 1.74083901202 -2.64636281352\n",
      "\n",
      "num = \n",
      "1201\n",
      "SENSOR 5 4.36236644466 -2.15541169775\n",
      "\n",
      "num = \n",
      "1202\n",
      "SENSOR 7 4.54743703627 -0.983253835746\n",
      "\n",
      "num = \n",
      "1203\n",
      "ODOMETRY 0.000431170979157 0.100025186646 -9.68768691897e-05\n",
      "\n",
      "sqP = \n",
      "[[1.430274749666687, 0.0, 0.0], [0.06007678167705366, 0.28420644732369216, 0.0], [0.005496073121263097, 0.02544345081499212, 0.291085011884246]]\n",
      "sigma = \n",
      "[[-0.74646592  7.77306173  1.25487577]\n",
      " [ 2.11408358  7.77306173  1.25487577]\n",
      " [-0.62631235  8.34147462  1.25487577]\n",
      " [-0.73547377  7.82394863  1.83704579]\n",
      " [-3.60701541  7.77306173  1.25487577]\n",
      " [-0.86661948  7.20464883  1.25487577]\n",
      " [-0.75745806  7.72217483  0.67270575]]\n",
      "sigmax = \n",
      "[[-0.74556731  7.846604    1.18690602]\n",
      " [ 2.11498219  7.72125672  1.34057637]\n",
      " [-0.62541374  8.42263385  1.19621119]\n",
      " [-0.73457516  7.89823358  1.76988853]\n",
      " [ 2.6770685   7.68355281  1.29978129]\n",
      " [-0.86572087  7.26951372  1.17858095]\n",
      " [-0.75655945  7.79496554  0.60393174]]\n",
      "sqP = \n",
      "[[1.4401168582303592, 0.0, 0.0], [-0.04770384095557056, 0.5342979218624849, 0.0], [0.05474613334580944, 0.014803050867737283, 0.5340367448252552]]\n",
      "m = \n",
      "[0.05812991 7.67316386 1.27791907]\n",
      "P = \n",
      "[[ 2.07008718 -0.05051888  0.05763664]\n",
      " [-0.05051888  0.08155038  0.00310756]\n",
      " [ 0.05763664  0.00310756  0.08458258]]\n",
      "num = \n",
      "1204\n",
      "SENSOR 8 2.94713147949 -0.538730214309\n",
      "\n",
      "num = \n",
      "1205\n",
      "SENSOR 4 1.59532499948 -2.64189895638\n",
      "\n",
      "num = \n",
      "1206\n",
      "SENSOR 5 4.3940471103 -2.19522353029\n",
      "\n",
      "num = \n",
      "1207\n",
      "SENSOR 7 4.58101568205 -0.938466709816\n",
      "\n",
      "num = \n",
      "1208\n",
      "ODOMETRY -0.00126089300156 0.100003408847 -0.000253955323998\n",
      "\n",
      "sqP = \n",
      "[[1.4387797527936168, 0.0, 0.0], [-0.03511230852891779, 0.28340343745549473, 0.0], [0.040059391998092886, 0.015928322937495864, 0.2876180074765287]]\n",
      "sigma = \n",
      "[[ 0.05812991  7.67316386  1.27791907]\n",
      " [ 2.93568941  7.67316386  1.27791907]\n",
      " [-0.01209471  8.23997073  1.27791907]\n",
      " [ 0.13824869  7.7050205   1.85315508]\n",
      " [-2.8194296   7.67316386  1.27791907]\n",
      " [ 0.12835453  7.10635698  1.27791907]\n",
      " [-0.02198888  7.64130721  0.70268305]]\n",
      "sigmax = \n",
      "[[ 0.0584642   7.77301758  1.2837733 ]\n",
      " [ 2.93602371  7.57524272  1.29832714]\n",
      " [-0.01176041  8.33998912  1.27675245]\n",
      " [ 0.13858299  7.80408538  1.86698214]\n",
      " [-2.8190953   7.57829835  1.24620828]\n",
      " [ 0.12868882  7.20555382  1.2907653 ]\n",
      " [-0.02165458  7.74130916  0.70052691]]\n",
      "sqP = \n",
      "[[1.5076222358888012, 0.0, 0.0], [-0.006917224172118739, 0.5365075014253067, 0.0], [0.02026486929968991, 0.007708810329042905, 0.5336434597223688]]\n",
      "m = \n",
      "[0.06275817 7.60430821 1.35303511]\n",
      "P = \n",
      "[[ 2.27259477 -0.00766185  0.02283785]\n",
      " [-0.00766185  0.07702561  0.00525468]\n",
      " [ 0.02283785  0.00525468  0.08352832]]\n",
      "num = \n",
      "1209\n",
      "SENSOR 8 3.01049631895 -0.647455670209\n",
      "\n",
      "num = \n",
      "1210\n",
      "SENSOR 4 1.74223188952 -2.64154107534\n",
      "\n",
      "num = \n",
      "1211\n",
      "SENSOR 5 4.6784242394 -2.17124070517\n",
      "\n",
      "num = \n",
      "1212\n",
      "SENSOR 7 4.2652733295 -0.996316702854\n",
      "\n",
      "num = \n",
      "1213\n",
      "ODOMETRY -0.000120654780893 0.0999758007847 -7.69033417334e-05\n",
      "\n",
      "sqP = \n",
      "[[1.5075127745821733, 0.0, 0.0], [-0.00508244592263004, 0.27748833276769846, 0.0], [0.01514935695639766, 0.019214038717148035, 0.2879750579647198]]\n",
      "sigma = \n",
      "[[ 0.06275817  7.60430821  1.35303511]\n",
      " [ 3.07778372  7.60430821  1.35303511]\n",
      " [ 0.05259328  8.15928488  1.35303511]\n",
      " [ 0.09305688  7.64273629  1.92898523]\n",
      " [-2.95226738  7.60430821  1.35303511]\n",
      " [ 0.07292306  7.04933155  1.35303511]\n",
      " [ 0.03245946  7.56588014  0.777085  ]]\n",
      "sigmax = \n",
      "[[ 0.06124332  7.70412258  1.35918117]\n",
      " [ 3.07626887  7.50451644  1.35953773]\n",
      " [ 0.05107843  8.25915656  1.35816627]\n",
      " [ 0.09154203  7.74231866  1.93815225]\n",
      " [-2.95378223  7.50606807  1.3343387 ]\n",
      " [ 0.07140821  7.14907828  1.36019544]\n",
      " [ 0.03094461  7.66583488  0.78020445]]\n",
      "sqP = \n",
      "[[1.5725298406711368, 0.0, 0.0], [-0.0010846108730862864, 0.5336599670012033, 0.0], [0.008829773947209409, 0.010976954886603389, 0.5326089369244731]]\n",
      "m = \n",
      "[0.0613514  7.43862442 1.33623629]\n",
      "P = \n",
      "[[ 2.47279438e+00 -1.28047387e-03  1.05847254e-02]\n",
      " [-1.28047387e-03  7.40451385e-02  5.65480622e-03]\n",
      " [ 1.05847254e-02  5.65480622e-03  8.52723512e-02]]\n",
      "num = \n",
      "1214\n",
      "SENSOR 8 2.81105029309 -0.668553762428\n",
      "\n",
      "num = \n",
      "1215\n",
      "SENSOR 4 1.84304791933 -2.65607644439\n",
      "\n",
      "num = \n",
      "1216\n",
      "SENSOR 5 4.63624935942 -2.13548023805\n",
      "\n",
      "num = \n",
      "1217\n",
      "SENSOR 7 4.52016763586 -0.991018307807\n",
      "\n",
      "num = \n",
      "1218\n",
      "ODOMETRY 0.000189591679111 0.100086348204 -0.000323351005088\n",
      "\n",
      "sqP = \n",
      "[[1.5725121249463971, 0.0, 0.0], [-0.0008142855335632691, 0.2721111454045902, 0.0], [0.006731093025600303, 0.020801379684505093, 0.29119468773701096]]\n",
      "sigma = \n",
      "[[ 0.0613514   7.43862442  1.33623629]\n",
      " [ 3.20637565  7.43862442  1.33623629]\n",
      " [ 0.05972283  7.98284671  1.33623629]\n",
      " [ 0.07481359  7.48022718  1.91862567]\n",
      " [-3.08367285  7.43862442  1.33623629]\n",
      " [ 0.06297997  6.89440213  1.33623629]\n",
      " [ 0.04788922  7.39702166  0.75384692]]\n",
      "sigmax = \n",
      "[[ 0.06115384  7.53841286  1.34235406]\n",
      " [-3.07700721  7.33885755  1.32977613]\n",
      " [ 0.05952527  8.08264498  1.34219154]\n",
      " [ 0.07461603  7.57992422  1.92608621]\n",
      " [-3.08387041  7.33881557  1.33046099]\n",
      " [ 0.06278241  6.99418048  1.34251657]\n",
      " [ 0.04769166  7.49688342  0.7586208 ]]\n",
      "sqP = \n",
      "[[1.4319915292493182, 0.0, 0.0], [0.08203716170988004, 0.5246104308581394, 0.0], [0.006404749147064916, 0.01137893728320876, 0.53389472000942]]\n",
      "m = \n",
      "[-0.74428665  7.36308245  1.2899737 ]\n",
      "P = \n",
      "[[2.04563028 0.0858023  0.00728689]\n",
      " [0.0858023  0.07944781 0.00556344]\n",
      " [0.00728689 0.00556344 0.08448514]]\n",
      "num = \n",
      "1219\n",
      "SENSOR 8 2.93905607509 -0.585176239548\n",
      "\n",
      "num = \n",
      "1220\n",
      "SENSOR 4 2.0438034489 -2.72800462462\n",
      "\n",
      "num = \n",
      "1221\n",
      "SENSOR 5 4.52648542497 -2.21078238898\n",
      "\n",
      "num = \n",
      "1222\n",
      "SENSOR 7 4.18292097158 -1.01609850061\n",
      "\n",
      "num = \n",
      "1223\n",
      "ODOMETRY -0.000473674854813 0.10002979275 -7.07593512027e-05\n",
      "\n",
      "sqP = \n",
      "[[1.4302553209975586, 0.0, 0.0], [0.0599908972151427, 0.2754067977993863, 0.0], [0.005094815822317256, 0.019091026462419485, 0.28999089396713884]]\n",
      "sigma = \n",
      "[[-0.74428665  7.36308245  1.2899737 ]\n",
      " [ 2.116224    7.36308245  1.2899737 ]\n",
      " [-0.62430485  7.91389605  1.2899737 ]\n",
      " [-0.73409701  7.40126451  1.86995549]\n",
      " [-3.60479729  7.36308245  1.2899737 ]\n",
      " [-0.86426844  6.81226886  1.2899737 ]\n",
      " [-0.75447628  7.3249004   0.70999191]]\n",
      "sigmax = \n",
      "[[-0.74442041  7.43671596  1.22218444]\n",
      " [ 2.11609024  7.31114306  1.37552823]\n",
      " [-0.62443861  7.99511417  1.23148529]\n",
      " [-0.73423077  7.47558492  1.80292003]\n",
      " [ 2.67825426  7.27353421  1.33467704]\n",
      " [-0.8644022   6.87725902  1.21385829]\n",
      " [-0.75461004  7.39783935  0.64145588]]\n",
      "sqP = \n",
      "[[1.4401136984320797, 0.0, 0.0], [-0.04818901524644314, 0.5292852101380165, 0.0], [0.054514659192631744, 0.011352748516982793, 0.5335165604484096]]\n",
      "m = \n",
      "[0.06323476 7.27444038 1.35908257]\n",
      "P = \n",
      "[[ 2.07005725e+00 -5.08784456e-02  5.75297760e-02]\n",
      " [-5.08784456e-02  7.71836429e-02  1.74213017e-03]\n",
      " [ 5.75297760e-02  1.74213017e-03  8.48269215e-02]]\n",
      "num = \n",
      "1224\n",
      "SENSOR 8 2.78937751626 -0.681683822875\n",
      "\n",
      "num = \n",
      "1225\n",
      "SENSOR 4 1.99021011507 -2.71901554183\n",
      "\n",
      "num = \n",
      "1226\n",
      "SENSOR 5 4.86030610026 -2.19809066259\n",
      "\n",
      "num = \n",
      "1227\n",
      "SENSOR 7 4.08317192213 -1.06263343315\n",
      "\n",
      "num = \n",
      "1228\n",
      "ODOMETRY -0.000361318872101 0.0998516170993 -0.000310786635761\n",
      "\n",
      "sqP = \n",
      "[[1.4387693529923429, 0.0, 0.0], [-0.03536247520285865, 0.2755596819018242, 0.0], [0.03998540548909549, 0.01145346467745091, 0.28826534134027343]]\n",
      "sigma = \n",
      "[[ 6.32347623e-02  7.27444038e+00  1.35908257e+00]\n",
      " [ 2.94077347e+00  7.27444038e+00  1.35908257e+00]\n",
      " [-7.49018808e-03  7.82555975e+00  1.35908257e+00]\n",
      " [ 1.43205573e-01  7.29734731e+00  1.93561326e+00]\n",
      " [-2.81430394e+00  7.27444038e+00  1.35908257e+00]\n",
      " [ 1.33959713e-01  6.72332102e+00  1.35908257e+00]\n",
      " [-1.67360487e-02  7.25153345e+00  7.82551892e-01]]\n",
      "sigmax = \n",
      "[[ 0.06269033  7.37427323  1.36535643]\n",
      " [ 2.94022903  7.1764303   1.37908216]\n",
      " [-0.00803462  7.92558637  1.35828596]\n",
      " [ 0.14266114  7.39635991  1.94984227]\n",
      " [-2.81484838  7.17970521  1.32697018]\n",
      " [ 0.13341528  6.82246094  1.37239554]\n",
      " [-0.01728048  7.35154843  0.78083049]]\n",
      "sqP = \n",
      "[[1.5076161940011805, 0.0, 0.0], [-0.006952909134654614, 0.5322944422680395, 0.0], [0.020267006846996456, 0.005418556148002237, 0.534024619492846]]\n",
      "m = \n",
      "[0.06463175 7.13865986 1.34930354]\n",
      "P = \n",
      "[[ 2.27257757 -0.00765557  0.02288505]\n",
      " [-0.00765557  0.07268143  0.00469952]\n",
      " [ 0.02288505  0.00469952  0.08490123]]\n",
      "num = \n",
      "1229\n",
      "SENSOR 8 2.73109069077 -0.664414233072\n",
      "\n",
      "num = \n",
      "1230\n",
      "SENSOR 4 2.02095934862 -2.77281077247\n",
      "\n",
      "num = \n",
      "1231\n",
      "SENSOR 5 4.87996286947 -2.20004470209\n",
      "\n",
      "num = \n",
      "1232\n",
      "SENSOR 7 4.22842676961 -1.08754115393\n",
      "\n",
      "num = \n",
      "1233\n",
      "ODOMETRY -0.000269192638065 0.100069154562 0.000122327434553\n",
      "\n",
      "sqP = \n",
      "[[1.507507070608945, 0.0, 0.0], [-0.00507829726269141, 0.2695470967850689, 0.0], [0.015180727503571498, 0.017720879558814528, 0.2904423304116762]]\n",
      "sigma = \n",
      "[[ 0.06463175  7.13865986  1.34930354]\n",
      " [ 3.07964589  7.13865986  1.34930354]\n",
      " [ 0.05447516  7.67775405  1.34930354]\n",
      " [ 0.09499321  7.17410162  1.9301882 ]\n",
      " [-2.95038239  7.13865986  1.34930354]\n",
      " [ 0.07478834  6.59956567  1.34930354]\n",
      " [ 0.0342703   7.1032181   0.76841888]]\n",
      "sigmax = \n",
      "[[ 0.06395964  7.23830532  1.35571663]\n",
      " [ 3.07897379  7.03900201  1.35552108]\n",
      " [ 0.05380305  7.77745951  1.35470426]\n",
      " [ 0.0943211   7.27350647  1.93962325]\n",
      " [-2.9510545   7.04062119  1.33036244]\n",
      " [ 0.07411624  6.69914085  1.35672834]\n",
      " [ 0.03359819  7.20301232  0.7718041 ]]\n",
      "sqP = \n",
      "[[1.5725246616592659, 0.0, 0.0], [-0.0010885052675648392, 0.5294987734437427, 0.0], [0.008849694417567566, 0.010370786797283156, 0.5339682934487905]]\n",
      "m = \n",
      "[0.06328913 7.09608638 1.30070613]\n",
      "P = \n",
      "[[ 2.47277738e+00 -1.29784529e-03  1.05706485e-02]\n",
      " [-1.29784529e-03  7.26369074e-02  4.68793661e-03]\n",
      " [ 1.05706485e-02  4.68793661e-03  8.38229649e-02]]\n",
      "num = \n",
      "1234\n",
      "SENSOR 8 2.66996787215 -0.741197858346\n",
      "\n",
      "num = \n",
      "1235\n",
      "SENSOR 1 4.92781804914 0.165464879583\n",
      "\n",
      "num = \n",
      "1236\n",
      "SENSOR 4 2.28452084059 -2.79103144813\n",
      "\n",
      "num = \n",
      "1237\n",
      "SENSOR 5 4.76222916544 -2.3384152959\n",
      "\n",
      "num = \n",
      "1238\n",
      "SENSOR 7 4.19121425588 -1.02506412666\n",
      "\n",
      "num = \n",
      "1239\n",
      "ODOMETRY 0.000939233063028 0.099978524037 0.000685447903704\n",
      "\n",
      "sqP = \n",
      "[[1.5725067176820782, 0.0, 0.0], [-0.0008253352911754599, 0.2695110874376153, 0.0], [0.006722164258648033, 0.017414810996933688, 0.2889195420923056]]\n",
      "sigma = \n",
      "[[ 0.06328913  7.09608638  1.30070613]\n",
      " [ 3.20830257  7.09608638  1.30070613]\n",
      " [ 0.06163846  7.63510856  1.30070613]\n",
      " [ 0.07673346  7.130916    1.87854521]\n",
      " [-3.0817243   7.09608638  1.30070613]\n",
      " [ 0.0649398   6.55706421  1.30070613]\n",
      " [ 0.0498448   7.06125676  0.72286705]]\n",
      "sigmax = \n",
      "[[ 0.06314227  7.19595689  1.30700831]\n",
      " [-3.07502961  6.99623802  1.29406235]\n",
      " [ 0.0614916   7.73498933  1.30684345]\n",
      " [ 0.0765866   7.23069276  1.88618948]\n",
      " [-3.08187117  6.9961949   1.29474562]\n",
      " [ 0.06479294  6.65692418  1.30717315]\n",
      " [ 0.04969794  7.16120297  0.727826  ]]\n",
      "sqP = \n",
      "[[1.4319914871646078, 0.0, 0.0], [0.08208804669665347, 0.5231469208449705, 0.0], [0.006543961553194323, 0.009449617399605469, 0.5326918199142612]]\n",
      "m = \n",
      "[-0.74947119  6.97201929  1.31305919]\n",
      "P = \n",
      "[[2.04514484e+00 8.29360475e-02 6.89472793e-03]\n",
      " [8.29360475e-02 6.00447008e-02 1.94218800e-03]\n",
      " [6.89472793e-03 1.94218800e-03 8.17146052e-02]]\n",
      "num = \n",
      "1240\n",
      "SENSOR 8 2.51553389791 -0.634533883027\n",
      "\n",
      "num = \n",
      "1241\n",
      "SENSOR 1 4.82706330783 0.0213662625167\n",
      "\n",
      "num = \n",
      "1242\n",
      "SENSOR 4 2.37505836382 -2.78320616069\n",
      "\n",
      "num = \n",
      "1243\n",
      "SENSOR 5 4.99415607057 -2.22355221099\n",
      "\n",
      "num = \n",
      "1244\n",
      "SENSOR 7 3.97703512937 -1.08364781079\n",
      "\n",
      "num = \n",
      "1245\n",
      "ODOMETRY -6.77419651127e-05 0.100140978268 -0.000135987996048\n",
      "\n",
      "sqP = \n",
      "[[1.4300856071518158, 0.0, 0.0], [0.05799376423734251, 0.23807860918723842, 0.0], [0.004821199439280716, 0.006983359411786985, 0.2857316816694636]]\n",
      "sigma = \n",
      "[[-0.74947119  6.97201929  1.31305919]\n",
      " [ 2.11070002  6.97201929  1.31305919]\n",
      " [-0.63348366  7.44817651  1.31305919]\n",
      " [-0.73982879  6.98598601  1.88452255]\n",
      " [-3.6096424   6.97201929  1.31305919]\n",
      " [-0.86545872  6.49586207  1.31305919]\n",
      " [-0.75911359  6.95805257  0.74159583]]\n",
      "sigmax = \n",
      "[[-0.74784651  7.04527243  1.24501742]\n",
      " [ 2.1123247   6.92054447  1.39876828]\n",
      " [-0.63185898  7.52881178  1.25395201]\n",
      " [-0.73820411  7.05989182  1.81719027]\n",
      " [ 2.67516758  6.88275115  1.35808034]\n",
      " [-0.86383404  6.56074871  1.23699718]\n",
      " [-0.75748891  7.03064623  0.67285089]]\n",
      "sqP = \n",
      "[[1.4400475174386358, 0.0, 0.0], [-0.0498340518309386, 0.5099900288423572, 0.0], [0.05464162927148493, 0.004612368170844654, 0.5313081659187159]]\n",
      "m = \n",
      "[0.06318876 6.80225468 1.33066914]\n",
      "P = \n",
      "[[ 2.06961496e+00 -5.16918556e-02  5.78499358e-02]\n",
      " [-5.16918556e-02  5.57217325e-02 -1.73154156e-03]\n",
      " [ 5.78499358e-02 -1.73154156e-03  8.30656073e-02]]\n",
      "num = \n",
      "1246\n",
      "SENSOR 8 2.23340888122 -0.783411064361\n",
      "\n",
      "num = \n",
      "1247\n",
      "SENSOR 1 4.76673370222 -0.00223170351281\n",
      "\n",
      "num = \n",
      "1248\n",
      "SENSOR 4 2.44121554947 -2.76116600902\n",
      "\n",
      "num = \n",
      "1249\n",
      "SENSOR 5 4.814470924 -2.31902709448\n",
      "\n",
      "num = \n",
      "1250\n",
      "SENSOR 7 4.04347080517 -1.14617801134\n",
      "\n",
      "num = \n",
      "1251\n",
      "ODOMETRY 0.00048886726123 0.100041297815 0.000679022990554\n",
      "\n",
      "sqP = \n",
      "[[1.4386156401243826, 0.0, 0.0], [-0.03593166525737609, 0.23330376741799194, 0.0], [0.04021222496145239, -0.0012286529333726142, 0.2853893387412426]]\n",
      "sigma = \n",
      "[[ 0.06318876  6.80225468  1.33066914]\n",
      " [ 2.94042004  6.80225468  1.33066914]\n",
      " [-0.00867457  7.26886222  1.33066914]\n",
      " [ 0.14361321  6.79979738  1.90144782]\n",
      " [-2.81404252  6.80225468  1.33066914]\n",
      " [ 0.13505209  6.33564715  1.33066914]\n",
      " [-0.01723569  6.80471199  0.75989046]]\n",
      "sigmax = \n",
      "[[ 0.06298503  6.90219623  1.33698594]\n",
      " [ 2.94021631  6.70413461  1.3506858 ]\n",
      " [-0.0088783   7.36899937  1.32979369]\n",
      " [ 0.14340948  6.89890841  1.91577329]\n",
      " [-2.81424625  6.70743568  1.29845777]\n",
      " [ 0.13484836  6.43487719  1.34414559]\n",
      " [-0.01743942  6.90483798  0.75815777]]\n",
      "sqP = \n",
      "[[1.5074889941936576, 0.0, 0.0], [-0.006393409880220532, 0.5115833485595482, 0.0], [0.02026582726495218, -0.0013523559633401747, 0.5324981418345363]]\n",
      "m = \n",
      "[0.06863654 6.7241576  1.44016749]\n",
      "P = \n",
      "[[ 2.27219550e+00 -6.97672281e-03  2.28108697e-02]\n",
      " [-6.97672281e-03  5.31553356e-02  6.06482021e-04]\n",
      " [ 2.28108697e-02  6.06482021e-04  8.09020839e-02]]\n",
      "num = \n",
      "1252\n",
      "SENSOR 8 2.30974435417 -0.73023844975\n",
      "\n",
      "num = \n",
      "1253\n",
      "SENSOR 1 4.628923258 0.0668774699138\n",
      "\n",
      "num = \n",
      "1254\n",
      "SENSOR 4 2.33202866839 -2.80658070702\n",
      "\n",
      "num = \n",
      "1255\n",
      "SENSOR 7 4.1171835927 -1.18735408115\n",
      "\n",
      "num = \n",
      "1256\n",
      "ODOMETRY -0.000763316825852 0.0998626212511 2.71380464145e-05\n",
      "\n",
      "sqP = \n",
      "[[1.507380343201222, 0.0, 0.0], [-0.004628375873011333, 0.23050794732835383, 0.0], [0.015132789688604568, 0.0029349194587136988, 0.284014909553761]]\n",
      "sigma = \n",
      "[[ 0.06863654  6.7241576   1.44016749]\n",
      " [ 3.08339723  6.7241576   1.44016749]\n",
      " [ 0.05937979  7.1851735   1.44016749]\n",
      " [ 0.09890212  6.73002744  2.00819731]\n",
      " [-2.94612415  6.7241576   1.44016749]\n",
      " [ 0.07789329  6.26314171  1.44016749]\n",
      " [ 0.03837096  6.71828776  0.87213767]]\n",
      "sigmax = \n",
      "[[ 0.06980443  6.82395998  1.44707738]\n",
      " [ 3.08456512  6.62428283  1.44593733]\n",
      " [ 0.06054768  7.28503556  1.44615325]\n",
      " [ 0.10007001  6.82957501  2.01812415]\n",
      " [-2.94495626  6.62603092  1.42068888]\n",
      " [ 0.07906118  6.36287585  1.44800092]\n",
      " [ 0.03953885  6.81825353  0.87602428]]\n",
      "sqP = \n",
      "[[1.5724013235355712, 0.0, 0.0], [-0.0010703086681655568, 0.510455654025525, 0.0], [0.00880034828690958, 0.0023925623622141736, 0.5305816709494064]]\n",
      "m = \n",
      "[0.06926353 6.70675668 1.40251564]\n",
      "P = \n",
      "[[ 2.47239650e+00 -1.00722162e-03  1.09448358e-02]\n",
      " [-1.00722162e-03  6.16897016e-02  1.68023674e-02]\n",
      " [ 1.09448358e-02  1.68023674e-02  1.09256833e-01]]\n",
      "num = \n",
      "1257\n",
      "SENSOR 8 2.30957175125 -0.879774630027\n",
      "\n",
      "num = \n",
      "1258\n",
      "SENSOR 1 4.47241468075 0.112481627522\n",
      "\n",
      "num = \n",
      "1259\n",
      "SENSOR 4 2.67200407606 -2.88213261922\n",
      "\n",
      "num = \n",
      "1260\n",
      "SENSOR 7 4.13385546866 -1.15907658911\n",
      "\n",
      "num = \n",
      "1261\n",
      "ODOMETRY 0.00049292915547 0.0998636041901 -0.000379476978871\n",
      "\n",
      "sqP = \n",
      "[[1.5723856068682858, 0.0, 0.0], [-0.0006405690935362965, 0.24837329013063494, 0.0], [0.0069606563110337465, 0.06766760717945916, 0.3234648003647235]]\n",
      "sigma = \n",
      "[[ 0.06926353  6.70675668  1.40251564]\n",
      " [ 3.21403474  6.70675668  1.40251564]\n",
      " [ 0.06798239  7.20350326  1.40251564]\n",
      " [ 0.08318484  6.84209189  2.04944524]\n",
      " [-3.07550768  6.70675668  1.40251564]\n",
      " [ 0.07054467  6.2100101   1.40251564]\n",
      " [ 0.05534222  6.57142147  0.75558604]]\n",
      "sigmax = \n",
      "[[ 0.06852735  6.8063851   1.4093509 ]\n",
      " [-3.06988674  6.60715049  1.39536374]\n",
      " [ 0.06724621  7.30314036  1.40922326]\n",
      " [ 0.08244866  6.94161551  2.05766675]\n",
      " [-3.07624386  6.60710704  1.39599709]\n",
      " [ 0.06980849  6.30962968  1.40947853]\n",
      " [ 0.05460604  6.67113539  0.76103372]]\n",
      "sqP = \n",
      "[[1.4319923924659828, 0.0, 0.0], [0.08218015339629521, 0.5166763435454532, 0.0], [0.007198778384444607, 0.04223700694989332, 0.5507066886796411]]\n",
      "m = \n",
      "[-0.75166533  6.5384531   1.28900127]\n",
      "P = \n",
      "[[2.04540208 0.08499194 0.0095036 ]\n",
      " [0.08499194 0.06820783 0.01839899]\n",
      " [0.0095036  0.01839899 0.11460061]]\n",
      "num = \n",
      "1262\n",
      "SENSOR 8 2.36124708193 -0.870145768712\n",
      "\n",
      "num = \n",
      "1263\n",
      "SENSOR 1 4.38735357247 0.0955180040226\n",
      "\n",
      "num = \n",
      "1264\n",
      "SENSOR 4 2.65985478012 -2.84310942138\n",
      "\n",
      "num = \n",
      "1265\n",
      "SENSOR 7 3.81181745959 -1.20782384771\n",
      "\n",
      "num = \n",
      "1266\n",
      "ODOMETRY 0.00033110801736 0.100044850743 -0.000577153724803\n",
      "\n",
      "sqP = \n",
      "[[1.430175542280612, 0.0, 0.0], [0.05942762670016284, 0.2543151427480041, 0.0], [0.0066450587310691155, 0.07079441601465251, 0.3309752367107339]]\n",
      "sigma = \n",
      "[[-0.75166533  6.5384531   1.28900127]\n",
      " [ 2.10868575  6.5384531   1.28900127]\n",
      " [-0.63281008  7.04708338  1.28900127]\n",
      " [-0.73837521  6.68004193  1.95095174]\n",
      " [-3.61201642  6.5384531   1.28900127]\n",
      " [-0.87052059  6.02982281  1.28900127]\n",
      " [-0.76495545  6.39686427  0.6270508 ]]\n",
      "sigmax = \n",
      "[[-0.75155188  6.61144233  1.22084475]\n",
      " [ 2.1087992   6.48724825  1.37473809]\n",
      " [-0.63269663  7.12763938  1.22998033]\n",
      " [-0.73826176  6.7539305   1.88377125]\n",
      " [ 2.67128234  6.44941475  1.33422197]\n",
      " [-0.87040713  6.09421541  1.21267085]\n",
      " [-0.764842    6.46894127  0.55793028]]\n",
      "sqP = \n",
      "[[1.4401044510251402, 0.0, 0.0], [-0.04839925483394942, 0.5228784051541314, 0.0], [0.055313675822778095, 0.045764725930736654, 0.5551338213213693]]\n",
      "m = \n",
      "[0.05599102 6.4550258  1.35682691]\n",
      "P = \n",
      "[[ 2.06992211 -0.05119056  0.05853869]\n",
      " [-0.05119056  0.06640579  0.01316431]\n",
      " [ 0.05853869  0.01316431  0.10947552]]\n",
      "num = \n",
      "1267\n",
      "SENSOR 8 2.41743004232 -0.864072811384\n",
      "\n",
      "num = \n",
      "1268\n",
      "SENSOR 1 4.40194504238 0.0658008097918\n",
      "\n",
      "num = \n",
      "1269\n",
      "SENSOR 4 2.94567913337 -2.88562727445\n",
      "\n",
      "num = \n",
      "1270\n",
      "SENSOR 7 3.82544464355 -1.2516790519\n",
      "\n",
      "num = \n",
      "1271\n",
      "ODOMETRY -0.000308672136194 0.0998384881725 2.64214654053e-05\n",
      "\n",
      "sqP = \n",
      "[[1.4387223864141878, 0.0, 0.0], [-0.035580566393033204, 0.2552250174767574, 0.0], [0.040687965564270165, 0.05725148035300509, 0.323329985231026]]\n",
      "sigma = \n",
      "[[ 0.05599102  6.4550258   1.35682691]\n",
      " [ 2.93343579  6.4550258   1.35682691]\n",
      " [-0.01517011  6.96547583  1.35682691]\n",
      " [ 0.13736695  6.56952876  2.00348688]\n",
      " [-2.82145375  6.4550258   1.35682691]\n",
      " [ 0.12715215  5.94457576  1.35682691]\n",
      " [-0.02538491  6.34052284  0.71016694]]\n",
      "sigmax = \n",
      "[[ 0.05574497  6.55491201  1.36245867]\n",
      " [ 2.93318975  6.35713373  1.37746946]\n",
      " [-0.01541616  7.06550967  1.3553424 ]\n",
      " [ 0.13712091  6.66862664  2.01721937]\n",
      " [-2.8216998   6.36007448  1.32531151]\n",
      " [ 0.12690611  6.04380875  1.36954643]\n",
      " [-0.02563096  6.44053629  0.7076607 ]]\n",
      "sqP = \n",
      "[[1.5075953012715129, 0.0, 0.0], [-0.005190898571730976, 0.5250510213181456, 0.0], [0.02136340335135582, 0.03479564058152516, 0.5529217170137365]]\n",
      "m = \n",
      "[0.05536036 6.36114449 1.31348686]\n",
      "P = \n",
      "[[ 2.27251571 -0.00582914  0.0243586 ]\n",
      " [-0.00582914  0.06452271  0.01583521]\n",
      " [ 0.0243586   0.01583521  0.10688421]]\n",
      "num = \n",
      "1272\n",
      "SENSOR 8 1.919908976 -0.934813972655\n",
      "\n",
      "num = \n",
      "1273\n",
      "SENSOR 1 4.19345581376 0.0757952107782\n",
      "\n",
      "num = \n",
      "1274\n",
      "SENSOR 4 2.94440761406 -2.83450142815\n",
      "\n",
      "num = \n",
      "1275\n",
      "SENSOR 7 3.92107607518 -1.15208824306\n",
      "\n",
      "num = \n",
      "1276\n",
      "ODOMETRY 5.25016195751e-05 0.0999941288634 0.000406623957959\n",
      "\n",
      "sqP = \n",
      "[[1.5074865553509316, 0.0, 0.0], [-0.0038667924521553158, 0.2539837764174203, 0.0], [0.016158422414433847, 0.06259332347047904, 0.32047650878327066]]\n",
      "sigma = \n",
      "[[ 0.05536036  6.36114449  1.31348686]\n",
      " [ 3.07033347  6.36114449  1.31348686]\n",
      " [ 0.04762677  6.86911205  1.31348686]\n",
      " [ 0.0876772   6.48633114  1.95443988]\n",
      " [-2.95961275  6.36114449  1.31348686]\n",
      " [ 0.06309394  5.85317694  1.31348686]\n",
      " [ 0.02304351  6.23595785  0.67253384]]\n",
      "sigmax = \n",
      "[[ 0.05507811  6.46083173  1.31898036]\n",
      " [ 3.07005122  6.26156158  1.32062599]\n",
      " [ 0.04734452  6.96883879  1.31820926]\n",
      " [ 0.08739495  6.58578882  1.96315153]\n",
      " [-2.959895    6.26294903  1.29544869]\n",
      " [ 0.06281169  5.95281871  1.31975113]\n",
      " [ 0.02276126  6.33577053  0.67480346]]\n",
      "sqP = \n",
      "[[1.5725112912989236, 0.0, 0.0], [-0.0003148478570523983, 0.5251834330068567, 0.0], [0.009344619667318364, 0.03892378518430118, 0.5497270955136838]]\n",
      "m = \n",
      "[0.05624585 6.19794902 1.37726791]\n",
      "P = \n",
      "[[ 2.47273348e+00 -4.61213292e-04  1.12757305e-02]\n",
      " [-4.61213292e-04  6.52647043e-02  1.50206275e-02]\n",
      " [ 1.12757305e-02  1.50206275e-02  1.02991409e-01]]\n",
      "num = \n",
      "1277\n",
      "SENSOR 8 1.9832859543 -0.98442650848\n",
      "\n",
      "num = \n",
      "1278\n",
      "SENSOR 1 4.09215025882 0.0717523426929\n",
      "\n",
      "num = \n",
      "1279\n",
      "SENSOR 4 3.05636372159 -2.93752404177\n",
      "\n",
      "num = \n",
      "1280\n",
      "SENSOR 7 3.89565529492 -1.28776086607\n",
      "\n",
      "num = \n",
      "1281\n",
      "ODOMETRY -0.000273197315602 0.0998069485111 -0.000285894067554\n",
      "\n",
      "sqP = \n",
      "[[1.572492760241813, 0.0, 0.0], [-0.00029330074119984784, 0.2554694077934284, 0.0], [0.0071706088417027415, 0.05880442113439375, 0.3154077218385558]]\n",
      "sigma = \n",
      "[[ 0.05624585  6.19794902  1.37726791]\n",
      " [ 3.20123137  6.19794902  1.37726791]\n",
      " [ 0.05565925  6.70888783  1.37726791]\n",
      " [ 0.07058707  6.31555786  2.00808336]\n",
      " [-3.08873967  6.19794902  1.37726791]\n",
      " [ 0.05683246  5.6870102   1.37726791]\n",
      " [ 0.04190464  6.08034018  0.74645247]]\n",
      "sigmax = \n",
      "[[ 0.05670498  6.29778472  1.38289445]\n",
      " [-3.08149481  6.09813298  1.37130269]\n",
      " [ 0.05611838  6.80872682  1.38283588]\n",
      " [ 0.0710462   6.41530261  2.01514103]\n",
      " [-3.08828054  6.0980948   1.37198014]\n",
      " [ 0.05729158  5.78684259  1.38295301]\n",
      " [ 0.04236376  6.1802463   0.75064671]]\n",
      "sqP = \n",
      "[[1.4319934380950146, 0.0, 0.0], [0.08237486883610107, 0.5190454080299958, 0.0], [0.006211612575551507, 0.035601561423745716, 0.5464904706987448]]\n",
      "m = \n",
      "[-0.75264844  6.09707951  1.35574918]\n",
      "P = \n",
      "[[2.04547475 0.08563949 0.00791556]\n",
      " [0.08563949 0.07257156 0.01411415]\n",
      " [0.00791556 0.01411415 0.09991941]]\n",
      "num = \n",
      "1282\n",
      "SENSOR 8 2.12296121839 -1.02645711564\n",
      "\n",
      "num = \n",
      "1283\n",
      "SENSOR 1 4.1786687521 0.0497541354582\n",
      "\n",
      "num = \n",
      "1284\n",
      "SENSOR 4 3.22093748114 -2.82397420009\n",
      "\n",
      "num = \n",
      "1285\n",
      "SENSOR 7 3.97449556221 -1.29780080623\n",
      "\n",
      "num = \n",
      "1286\n",
      "ODOMETRY -0.000265608833175 0.100026175106 0.000137251905213\n",
      "\n",
      "sqP = \n",
      "[[1.4302009458798062, 0.0, 0.0], [0.05987934226058011, 0.26265190208017847, 0.0], [0.005534579769778138, 0.052475302662574344, 0.31166508621787714]]\n",
      "sigma = \n",
      "[[-0.75264844  6.09707951  1.35574918]\n",
      " [ 2.10775345  6.09707951  1.35574918]\n",
      " [-0.63288975  6.62238331  1.35574918]\n",
      " [-0.74157928  6.20203011  1.97907936]\n",
      " [-3.61305033  6.09707951  1.35574918]\n",
      " [-0.87240712  5.5717757   1.35574918]\n",
      " [-0.7637176   5.9921289   0.73241901]]\n",
      "sigmax = \n",
      "[[-0.75320753  6.16990806  1.28750383]\n",
      " [ 2.10719436  6.04604932  1.44152416]\n",
      " [-0.63344885  6.70284369  1.29669366]\n",
      " [-0.74213837  6.27560961  1.91164432]\n",
      " [ 2.66957589  6.00817317  1.40110433]\n",
      " [-0.87296621  5.63592917  1.27929162]\n",
      " [-0.76427669  6.06419759  0.6633717 ]]\n",
      "sqP = \n",
      "[[1.440115986141985, 0.0, 0.0], [-0.04796712306819441, 0.5249194020228651, 0.0], [0.0550587640055015, 0.03203710401673589, 0.5447923961340257]]\n",
      "m = \n",
      "[0.0306666  6.05968853 1.24289587]\n",
      "P = \n",
      "[[ 2.06999501 -0.05057538  0.05816821]\n",
      " [-0.05057538  0.06991108  0.00949883]\n",
      " [ 0.05816821  0.00949883  0.09838855]]\n",
      "num = \n",
      "1287\n",
      "SENSOR 8 2.05703216981 -1.00685667222\n",
      "\n",
      "num = \n",
      "1288\n",
      "SENSOR 1 3.94149875624 0.140569733461\n",
      "\n",
      "num = \n",
      "1289\n",
      "SENSOR 4 3.13725363665 -2.87162377088\n",
      "\n",
      "num = \n",
      "1290\n",
      "SENSOR 7 3.76919105935 -1.28829235854\n",
      "\n",
      "num = \n",
      "1291\n",
      "ODOMETRY 0.000720910278101 0.100023575271 -0.000251711733041\n",
      "\n",
      "sqP = \n",
      "[[1.438747722294526, 0.0, 0.0], [-0.035152363467668736, 0.26205989318085277, 0.0], [0.04042975180913639, 0.04166997392402103, 0.3082492543062793]]\n",
      "sigma = \n",
      "[[ 0.0306666   6.05968853  1.24289587]\n",
      " [ 2.90816204  6.05968853  1.24289587]\n",
      " [-0.03963813  6.58380831  1.24289587]\n",
      " [ 0.1115261   6.14302847  1.85939438]\n",
      " [-2.84682885  6.05968853  1.24289587]\n",
      " [ 0.10097132  5.53556874  1.24289587]\n",
      " [-0.05019291  5.97634858  0.62639737]]\n",
      "sigmax = \n",
      "[[ 0.03053824  6.15966848  1.2459363 ]\n",
      " [ 2.90803368  5.96238135  1.26605942]\n",
      " [-0.03976649  6.68375486  1.23890552]\n",
      " [ 0.11139774  6.24243618  1.8705004 ]\n",
      " [-2.84695721  5.96396869  1.21386231]\n",
      " [ 0.10084297  5.63508813  1.25295206]\n",
      " [-0.05032126  6.07624744  0.62135233]]\n",
      "sqP = \n",
      "[[1.5076024880240806, 0.0, 0.0], [-0.005377398297130622, 0.5269288623015536, 0.0], [0.020909854037150222, 0.02349652354806467, 0.5447230201471666]]\n",
      "m = \n",
      "[0.0346113  5.98259269 1.31574908]\n",
      "P = \n",
      "[[ 2.27253562 -0.00601795  0.02358281]\n",
      " [-0.00601795  0.06882861  0.01117752]\n",
      " [ 0.02358281  0.01117752  0.09290158]]\n",
      "num = \n",
      "1292\n",
      "SENSOR 8 1.81579376561 -1.08266616382\n",
      "\n",
      "num = \n",
      "1293\n",
      "SENSOR 1 3.67093999662 0.0393555510994\n",
      "\n",
      "num = \n",
      "1294\n",
      "SENSOR 4 3.34596406803 -2.97433102412\n",
      "\n",
      "num = \n",
      "1295\n",
      "SENSOR 7 3.90187420479 -1.31346748714\n",
      "\n",
      "num = \n",
      "1296\n",
      "ODOMETRY 0.000105339136838 0.099882486807 0.000711003741797\n",
      "\n",
      "sqP = \n",
      "[[1.5074931591349836, 0.0, 0.0], [-0.003992022344677905, 0.2623216971324861, 0.0], [0.015643726024921478, 0.04284802168011864, 0.30136505770318456]]\n",
      "sigma = \n",
      "[[ 3.46113020e-02  5.98259269e+00  1.31574908e+00]\n",
      " [ 3.04959762e+00  5.98259269e+00  1.31574908e+00]\n",
      " [ 2.66272573e-02  6.50723608e+00  1.31574908e+00]\n",
      " [ 6.58987540e-02  6.06828873e+00  1.91847920e+00]\n",
      " [-2.98037502e+00  5.98259269e+00  1.31574908e+00]\n",
      " [ 4.25953467e-02  5.45794929e+00  1.31574908e+00]\n",
      " [ 3.32384992e-03  5.89689665e+00  7.13018969e-01]]\n",
      "sigmax = \n",
      "[[ 3.50805005e-02  6.08255384e+00  1.31928240e+00]\n",
      " [ 3.05006682e+00  5.88298547e+00  1.32486598e+00]\n",
      " [ 2.70964558e-02  6.60722226e+00  1.31848420e+00]\n",
      " [ 6.63679526e-02  6.16809043e+00  1.92513781e+00]\n",
      " [-2.97990582e+00  5.88387776e+00  1.29962211e+00]\n",
      " [ 4.30645452e-02  5.55787905e+00  1.32008038e+00]\n",
      " [ 3.79304846e-03  5.99691940e+00  7.13423539e-01]]\n",
      "sqP = \n",
      "[[1.5725127303920676, 0.0, 0.0], [-0.0004541098863876093, 0.5273368477848447, 0.0], [0.009064640361247394, 0.024887143639863787, 0.5395493060882045]]\n",
      "m = \n",
      "[0.03505008 5.77396637 1.30248241]\n",
      "P = \n",
      "[[ 2.47273952e+00 -5.72582065e-04  1.08907434e-02]\n",
      " [-5.72582065e-04  6.88655399e-02  1.08243916e-02]\n",
      " [ 1.08907434e-02  1.08243916e-02  9.19650533e-02]]\n",
      "num = \n",
      "1297\n",
      "SENSOR 8 1.81140030994 -1.14228995915\n",
      "\n",
      "num = \n",
      "1298\n",
      "SENSOR 1 3.8261635831 0.054006173831\n",
      "\n",
      "num = \n",
      "1299\n",
      "SENSOR 4 3.4796093187 -2.98658709002\n",
      "\n",
      "num = \n",
      "1300\n",
      "SENSOR 7 3.82419437586 -1.32957085243\n",
      "\n",
      "num = \n",
      "1301\n",
      "ODOMETRY -7.64579991343e-05 0.0997922673878 -0.000288040106145\n",
      "\n",
      "sqP = \n",
      "[[1.5724946814987204, 0.0, 0.0], [-0.00036412337138592646, 0.26242219285221924, 0.0], [0.006925774364389466, 0.04125761346435929, 0.30035794693481166]]\n",
      "sigma = \n",
      "[[ 0.03505008  5.77396637  1.30248241]\n",
      " [ 3.18003945  5.77396637  1.30248241]\n",
      " [ 0.03432184  6.29881075  1.30248241]\n",
      " [ 0.04890163  5.85648159  1.90319831]\n",
      " [-3.10993928  5.77396637  1.30248241]\n",
      " [ 0.03577833  5.24912198  1.30248241]\n",
      " [ 0.02119853  5.69145114  0.70176652]]\n",
      "sigmax = \n",
      "[[ 0.03586643  5.87378714  1.3059931 ]\n",
      " [-3.10232952  5.6741581   1.29863269]\n",
      " [ 0.03513818  6.39863405  1.30592041]\n",
      " [ 0.04971798  5.95624416  1.90809129]\n",
      " [-3.10912294  5.67413425  1.29931081]\n",
      " [ 0.03659467  5.34894017  1.3060658 ]\n",
      " [ 0.02201488  5.79131096  0.70389424]]\n",
      "sqP = \n",
      "[[1.4319922515674903, 0.0, 0.0], [0.08225350257529336, 0.5208392089329832, 0.0], [0.0043440025142290565, 0.023633446279585024, 0.5385745266844709]]\n",
      "m = \n",
      "[-0.76476141  5.72726958  1.32770492]\n",
      "P = \n",
      "[[2.04558023 0.08599595 0.00527346]\n",
      " [0.08599595 0.07677088 0.00813219]\n",
      " [0.00527346 0.00813219 0.08722249]]\n",
      "num = \n",
      "1302\n",
      "SENSOR 8 1.97927232141 -1.27560024901\n",
      "\n",
      "num = \n",
      "1303\n",
      "SENSOR 1 3.72258806814 0.161210114162\n",
      "\n",
      "num = \n",
      "1304\n",
      "SENSOR 4 3.3133245266 -2.89891758969\n",
      "\n",
      "num = \n",
      "1305\n",
      "SENSOR 7 3.81183963458 -1.32175712764\n",
      "\n",
      "num = \n",
      "1306\n",
      "ODOMETRY 0.000385928532719 0.0996806600309 -0.200347125045\n",
      "\n",
      "sqP = \n",
      "[[1.4302378235236293, 0.0, 0.0], [0.060127030838222045, 0.27047296518816005, 0.0], [0.003687119390721551, 0.02924688079754262, 0.29385969381907445]]\n",
      "sigma = \n",
      "[[-0.76476141  5.72726958  1.32770492]\n",
      " [ 2.09571424  5.72726958  1.32770492]\n",
      " [-0.64450735  6.26821551  1.32770492]\n",
      " [-0.75738717  5.78576334  1.91542431]\n",
      " [-3.62523706  5.72726958  1.32770492]\n",
      " [-0.88501547  5.18632365  1.32770492]\n",
      " [-0.77213565  5.66877582  0.73998553]]\n",
      "sigmax = \n",
      "[[-0.76512591  5.79926917  1.25860676]\n",
      " [ 2.09534974  5.67726609  1.41406549]\n",
      " [-0.64487185  6.34798445  1.26774316]\n",
      " [-0.75775167  5.85827051  1.84685896]\n",
      " [ 2.65758375  5.63892643  1.37411594]\n",
      " [-0.88537997  5.24951395  1.25046838]\n",
      " [-0.77250015  5.74026391  0.67035831]]\n",
      "sqP = \n",
      "[[1.4401167913516375, 0.0, 0.0], [-0.04706684298499021, 0.5271522049274567, 0.0], [0.05530705812682104, 0.017043589120588773, 0.5354549011946909]]\n",
      "m = \n",
      "[0.02093787 5.74867612 1.28611305]\n",
      "P = \n",
      "[[ 2.07005829 -0.04955603  0.05844105]\n",
      " [-0.04955603  0.07335835  0.00417987]\n",
      " [ 0.05844105  0.00417987  0.08838623]]\n",
      "num = \n",
      "1307\n",
      "SENSOR 8 1.71109074699 -1.02714696631\n",
      "\n",
      "num = \n",
      "1308\n",
      "SENSOR 1 3.47474230224 0.283685656426\n",
      "\n",
      "num = \n",
      "1309\n",
      "SENSOR 4 3.49665186449 -2.71608754934\n",
      "\n",
      "num = \n",
      "1310\n",
      "SENSOR 7 3.86100523492 -1.24660856692\n",
      "\n",
      "num = \n",
      "1311\n",
      "ODOMETRY 1.18794066805e-06 0.0997733761035 -0.199977592699\n",
      "\n",
      "sqP = \n",
      "[[1.4387697137946382, 0.0, 0.0], [-0.034443338963676906, 0.2686484816633867, 0.0], [0.04061876527579112, 0.02076661531755807, 0.29377728334230974]]\n",
      "sigma = \n",
      "[[ 0.02093787  5.74867612  1.28611305]\n",
      " [ 2.89847729  5.74867612  1.28611305]\n",
      " [-0.04794881  6.28597308  1.28611305]\n",
      " [ 0.1021754   5.79020935  1.87366761]\n",
      " [-2.85660156  5.74867612  1.28611305]\n",
      " [ 0.08982454  5.21137916  1.28611305]\n",
      " [-0.06029967  5.70714289  0.69855848]]\n",
      "sigmax = \n",
      "[[-0.17902333  5.84833412  1.28823846]\n",
      " [ 2.6985161   5.65191754  1.31007159]\n",
      " [-0.24791001  6.38554101  1.28137373]\n",
      " [-0.0977858   5.88936621  1.88387308]\n",
      " [-3.05656276  5.65302699  1.25805102]\n",
      " [-0.11013665  5.31065449  1.29509309]\n",
      " [-0.26026086  5.80664469  0.69258981]]\n",
      "sqP = \n",
      "[[1.5076121905265296, 0.0, 0.0], [-0.0058468068725035394, 0.528946047920152, 0.0], [0.02051366526859322, 0.010410919886638626, 0.5369843612721809]]\n",
      "m = \n",
      "[-0.17546717  5.57963284  1.31657568]\n",
      "P = \n",
      "[[ 2.27256847 -0.00644936  0.02311987]\n",
      " [-0.00644936  0.07122899  0.00694043]\n",
      " [ 0.02311987  0.00694043  0.08611093]]\n",
      "num = \n",
      "1312\n",
      "SENSOR 8 1.7913022851 -0.988844108691\n",
      "\n",
      "num = \n",
      "1313\n",
      "SENSOR 1 3.3603196986 0.438921773545\n",
      "\n",
      "num = \n",
      "1314\n",
      "SENSOR 4 3.59460143268 -2.4995361102\n",
      "\n",
      "num = \n",
      "1315\n",
      "SENSOR 7 3.6769689176 -1.0092059111\n",
      "\n",
      "num = \n",
      "1316\n",
      "ODOMETRY -0.000816556751862 0.0997465372851 -0.202314638633\n",
      "\n",
      "sqP = \n",
      "[[1.5075040518244616, 0.0, 0.0], [-0.0042781684811384595, 0.2668533085192455, 0.0], [0.015336520302255591, 0.02625427042105714, 0.29186714769330974]]\n",
      "sigma = \n",
      "[[-0.17546717  5.57963284  1.31657568]\n",
      " [ 2.83954094  5.57963284  1.31657568]\n",
      " [-0.1840235   6.11333946  1.31657568]\n",
      " [-0.14479413  5.63214138  1.90030998]\n",
      " [-3.19047527  5.57963284  1.31657568]\n",
      " [-0.16691083  5.04592623  1.31657568]\n",
      " [-0.20614021  5.5271243   0.73284139]]\n",
      "sigmax = \n",
      "[[-0.37544357  5.67787423  1.29915855]\n",
      " [ 2.63956453  5.48437636  1.34625612]\n",
      " [-0.38399991  6.21142822  1.29831861]\n",
      " [-0.34477053  5.73087071  1.88591393]\n",
      " [ 2.89273363  5.47997864  1.32145081]\n",
      " [-0.36688723  5.14431304  1.29999976]\n",
      " [-0.40611661  5.62478532  0.71241956]]\n",
      "sqP = \n",
      "[[1.4334609338435846, 0.0, 0.0], [-0.08097135507868028, 0.5220140468382123, 0.0], [0.01712212084679599, 0.01492087948585214, 0.5346181447155732]]\n",
      "m = \n",
      "[0.43731141 5.46256012 1.3504533 ]\n",
      "P = \n",
      "[[ 2.04974503 -0.08502843  0.01765735]\n",
      " [-0.08502843  0.07819208  0.0032205 ]\n",
      " [ 0.01765735  0.0032205   0.08333625]]\n",
      "num = \n",
      "1317\n",
      "SENSOR 8 1.72700243897 -0.727469713931\n",
      "\n",
      "num = \n",
      "1318\n",
      "SENSOR 1 3.41603446384 0.795914407086\n",
      "\n",
      "num = \n",
      "1319\n",
      "SENSOR 4 3.74498201327 -2.35790495186\n",
      "\n",
      "num = \n",
      "1320\n",
      "SENSOR 7 3.65858016772 -0.852210047465\n",
      "\n",
      "num = \n",
      "1321\n",
      "ODOMETRY -0.00017107666944 0.100202752156 -0.199982427928\n",
      "\n",
      "sqP = \n",
      "[[1.4316930650730926, 0.0, 0.0], [-0.05939012647560663, 0.2732487795864036, 0.0], [0.012333193192433988, 0.01446657077855152, 0.28805357902938816]]\n",
      "sigma = \n",
      "[[ 0.43731141  5.46256012  1.3504533 ]\n",
      " [ 3.30069754  5.46256012  1.3504533 ]\n",
      " [ 0.31853116  6.00905768  1.3504533 ]\n",
      " [ 0.46197779  5.49149326  1.92656046]\n",
      " [-2.42607472  5.46256012  1.3504533 ]\n",
      " [ 0.55609166  4.91606256  1.3504533 ]\n",
      " [ 0.41264502  5.43362698  0.77434614]]\n",
      "sigmax = \n",
      "[[ 0.23418021  5.55295433  1.3926227 ]\n",
      " [ 3.09756634  5.36406056  1.33473044]\n",
      " [ 0.11539996  6.10381209  1.38161375]\n",
      " [ 0.2588466   5.58081991  1.9709465 ]\n",
      " [-2.62920592  5.38722266  1.28508023]\n",
      " [ 0.35296047  5.00082273  1.40303739]\n",
      " [ 0.20951383  5.52503375  0.81427324]]\n",
      "sqP = \n",
      "[[1.5011410084850447, 0.0, 0.0], [-0.016317532968135484, 0.5309224807537205, 0.0], [0.014425974341555823, 0.010289145272471757, 0.5336354686061827]]\n",
      "m = \n",
      "[0.23827626 5.3900202  1.38836793]\n",
      "P = \n",
      "[[ 2.25307704 -0.01815345  0.01613946]\n",
      " [-0.01815345  0.07324156  0.0023664 ]\n",
      " [ 0.01613946  0.0023664   0.08223221]]\n",
      "num = \n",
      "1322\n",
      "SENSOR 8 1.59030504807 -0.562457859173\n",
      "\n",
      "num = \n",
      "1323\n",
      "SENSOR 1 3.26977624121 1.02025691453\n",
      "\n",
      "num = \n",
      "1324\n",
      "SENSOR 4 3.98177703193 -2.11166828345\n",
      "\n",
      "num = \n",
      "1325\n",
      "SENSOR 7 3.60670130028 -0.68064217983\n",
      "\n",
      "num = \n",
      "1326\n",
      "ODOMETRY 0.000590926276013 0.100545969921 -0.199693808091\n",
      "\n",
      "sqP = \n",
      "[[1.5010253305651218, 0.0, 0.0], [-0.012094035782492567, 0.2703614131865276, 0.0], [0.010752289763767511, 0.009233719502033938, 0.2864111263972287]]\n",
      "sigma = \n",
      "[[ 0.23827626  5.3900202   1.38836793]\n",
      " [ 3.24032693  5.3900202   1.38836793]\n",
      " [ 0.21408819  5.93074303  1.38836793]\n",
      " [ 0.25978084  5.40848764  1.96119018]\n",
      " [-2.7637744   5.3900202   1.38836793]\n",
      " [ 0.26246434  4.84929738  1.38836793]\n",
      " [ 0.21677169  5.37155276  0.81554567]]\n",
      "sigmax = \n",
      "[[ 0.03812276  5.4873959   1.41200192]\n",
      " [ 3.04017342  5.29030378  1.37850761]\n",
      " [ 0.01393469  6.02866184  1.4096399 ]\n",
      " [ 0.05962734  5.50533262  1.98691257]\n",
      " [-2.9639279   5.29687826  1.35141972]\n",
      " [ 0.06231083  4.94607298  1.41435011]\n",
      " [ 0.01661818  5.46941414  0.83708034]]\n",
      "sqP = \n",
      "[[1.5663138001163128, 0.0, 0.0], [-0.0036032257521233435, 0.5294918261558742, 0.0], [0.008472105083650499, 0.007537331574742422, 0.5319450933269175]]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "m = \n",
      "[0.04021417 5.22129    1.43833099]\n",
      "P = \n",
      "[[ 2.45327809e+00 -4.27793599e-03  1.00535097e-02]\n",
      " [-4.27793599e-03  7.33060144e-02  1.26003681e-03]\n",
      " [ 1.00535097e-02  1.26003681e-03  8.12077454e-02]]\n",
      "num = \n",
      "1327\n",
      "SENSOR 8 1.42956896486 -0.405210773852\n",
      "\n",
      "num = \n",
      "1328\n",
      "SENSOR 1 3.11530262765 1.08617098938\n",
      "\n",
      "num = \n",
      "1329\n",
      "SENSOR 4 3.75792044617 -1.94960186471\n",
      "\n",
      "num = \n",
      "1330\n",
      "SENSOR 7 3.44632621596 -0.561375582074\n",
      "\n",
      "num = \n",
      "1331\n",
      "ODOMETRY -0.000298257136816 0.0996631664365 -0.198915852019\n",
      "\n",
      "sqP = \n",
      "[[1.5662943810446885, 0.0, 0.0], [-0.0027312464624234547, 0.27073705817579113, 0.0], [0.006418659141900886, 0.004718850667456156, 0.28485834839071816]]\n",
      "sigma = \n",
      "[[ 0.04021417  5.22129     1.43833099]\n",
      " [ 3.17280294  5.22129     1.43833099]\n",
      " [ 0.03475168  5.76276412  1.43833099]\n",
      " [ 0.05305149  5.2307277   2.00804769]\n",
      " [-3.09237459  5.22129     1.43833099]\n",
      " [ 0.04567667  4.67981588  1.43833099]\n",
      " [ 0.02737686  5.2118523   0.8686143 ]]\n",
      "sigmax = \n",
      "[[-0.15888871  5.32175227  1.44243264]\n",
      " [ 2.97370005  5.12079487  1.43513405]\n",
      " [-0.1643512   5.8632473   1.44188381]\n",
      " [-0.14605139  5.33112904  2.01343863]\n",
      " [ 2.99170784  5.12086873  1.43332497]\n",
      " [-0.15342621  4.78025425  1.44298135]\n",
      " [-0.17172603  5.31235895  0.87142598]]\n",
      "sqP = \n",
      "[[1.431998516623607, 0.0, 0.0], [-0.0831420466499553, 0.523433844858543, 0.0], [-0.0020954668075476293, 0.0026750936322672454, 0.530578369312128]]\n",
      "m = \n",
      "[0.63941375 5.18758597 1.57622539]\n",
      "P = \n",
      "[[ 2.04556482e+00 -8.72586039e-02 -1.81898025e-03]\n",
      " [-8.72586039e-02  8.07574259e-02 -1.85836762e-03]\n",
      " [-1.81898025e-03 -1.85836762e-03  7.93825869e-02]]\n",
      "num = \n",
      "1332\n",
      "SENSOR 8 1.41119753152 -0.218291775829\n",
      "\n",
      "num = \n",
      "1333\n",
      "SENSOR 1 3.14171978452 1.37417197214\n",
      "\n",
      "num = \n",
      "1334\n",
      "SENSOR 4 3.97964357546 -1.84339806385\n",
      "\n",
      "num = \n",
      "1335\n",
      "SENSOR 7 3.37769084878 -0.326486102096\n",
      "\n",
      "num = \n",
      "1336\n",
      "ODOMETRY -0.000223643982462 0.0996312038275 -0.2025564277\n",
      "\n",
      "sqP = \n",
      "[[1.4302324362013519, 0.0, 0.0], [-0.061010085982098915, 0.27755214880436535, 0.0], [-0.0012718074406234438, -0.006975124164884726, 0.2816599315456835]]\n",
      "sigma = \n",
      "[[ 0.63941375  5.18758597  1.57622539]\n",
      " [ 3.49987862  5.18758597  1.57622539]\n",
      " [ 0.51739358  5.74269026  1.57622539]\n",
      " [ 0.63687014  5.17363572  2.13954525]\n",
      " [-2.22105112  5.18758597  1.57622539]\n",
      " [ 0.76143392  4.63248167  1.57622539]\n",
      " [ 0.64195737  5.20153622  1.01290553]]\n",
      "sigmax = \n",
      "[[ 0.44019964  5.26757798  1.63567305]\n",
      " [-2.98252079  5.09424107  1.54130439]\n",
      " [ 0.31817947  5.82932335  1.62549461]\n",
      " [ 0.43765603  5.25377869  2.19878925]\n",
      " [-2.42026523  5.1272273   1.49691853]\n",
      " [ 0.56221981  4.7046431   1.64496748]\n",
      " [ 0.44274326  5.28137676  1.07255646]]\n",
      "sqP = \n",
      "[[1.4401502979303091, 0.0, 0.0], [0.052970530724105, 0.5300634228240168, 0.0], [0.04649013264741024, -0.0046150706255305065, 0.5289426246610703]]\n",
      "m = \n",
      "[-0.35454946  5.0974768   1.64826734]\n",
      "P = \n",
      "[[ 2.07038575e+00  5.55972291e-02  4.91562693e-02]\n",
      " [ 5.55972291e-02  7.75350936e-02 -8.67230780e-04]\n",
      " [ 4.91562693e-02 -8.67230780e-04  8.24663655e-02]]\n",
      "num = \n",
      "1337\n",
      "SENSOR 8 1.25280389716 -0.0478523885333\n",
      "\n",
      "num = \n",
      "1338\n",
      "SENSOR 1 3.28031792034 1.55461890617\n",
      "\n",
      "num = \n",
      "1339\n",
      "SENSOR 4 3.96871903979 -1.63320364602\n",
      "\n",
      "num = \n",
      "1340\n",
      "SENSOR 7 3.38773338161 -0.165645510408\n",
      "\n",
      "num = \n",
      "1341\n",
      "ODOMETRY -0.000881424513507 0.0999464043441 -0.148617707433\n",
      "\n",
      "sqP = \n",
      "[[1.4388835082988523, 0.0, 0.0], [0.03863914540848221, 0.2757573390911632, 0.0], [0.03416278592349643, -0.007931798443521269, 0.2850199222988574]]\n",
      "sigma = \n",
      "[[-0.35454946  5.0974768   1.64826734]\n",
      " [ 2.52321755  5.0974768   1.64826734]\n",
      " [-0.27727117  5.64899148  1.64826734]\n",
      " [-0.28622389  5.0816132   2.21830719]\n",
      " [-3.23231648  5.0974768   1.64826734]\n",
      " [-0.43182776  4.54596212  1.64826734]\n",
      " [-0.42287504  5.1133404   1.0782275 ]]\n",
      "sigmax = \n",
      "[[-0.55732954  5.1909035   1.61365769]\n",
      " [ 2.32043748  5.01630807  1.7060429 ]\n",
      " [-0.48005125  5.74481127  1.62097366]\n",
      " [-0.48900396  5.1771848   2.19015676]\n",
      " [ 2.84808875  4.99825736  1.65731606]\n",
      " [-0.63460783  4.63643808  1.60654831]\n",
      " [-0.62565511  5.20418623  1.03732014]]\n",
      "sqP = \n",
      "[[1.4389620899758886, 0.0, 0.0], [-0.06823200225131516, 0.5277324074532248, 0.0], [0.033642617758295884, -0.001768783273421722, 0.5319369848384418]]\n",
      "m = \n",
      "[0.23621739 5.12182246 1.71298734]\n",
      "P = \n",
      "[[ 2.06647881 -0.07214924  0.0361989 ]\n",
      " [-0.07214924  0.07980733 -0.00733046]\n",
      " [ 0.0361989  -0.00733046  0.0812466 ]]\n",
      "num = \n",
      "1342\n",
      "SENSOR 8 1.21794802013 0.101499246355\n",
      "\n",
      "num = \n",
      "1343\n",
      "SENSOR 1 3.22237509743 1.79072716463\n",
      "\n",
      "num = \n",
      "1344\n",
      "SENSOR 4 3.95617109814 -1.44625205115\n",
      "\n",
      "num = \n",
      "1345\n",
      "SENSOR 7 3.44736574276 0.0259459077986\n",
      "\n",
      "num = \n",
      "1346\n",
      "ODOMETRY 6.73076006284e-05 0.100132073471 -0.000119445516902\n",
      "\n",
      "sqP = \n",
      "[[1.4375252377708532, 0.0, 0.0], [-0.05018989596659442, 0.2780077478988693, 0.0], [0.02518140350383906, -0.021821723968124318, 0.28308357013132046]]\n",
      "sigma = \n",
      "[[ 0.23621739  5.12182246  1.71298734]\n",
      " [ 3.11126787  5.12182246  1.71298734]\n",
      " [ 0.1358376   5.67783795  1.71298734]\n",
      " [ 0.2865802   5.07817901  2.27915448]\n",
      " [-2.63883309  5.12182246  1.71298734]\n",
      " [ 0.33659718  4.56580696  1.71298734]\n",
      " [ 0.18585458  5.1654659   1.1468202 ]]\n",
      "sigmax = \n",
      "[[ 0.08671826  5.21901394  1.73629181]\n",
      " [ 2.96176873  5.02192471  1.71610578]\n",
      " [-0.01366153  5.77687557  1.72643482]\n",
      " [ 0.13708107  5.17407408  2.30732217]\n",
      " [-2.78833222  5.03420138  1.66490583]\n",
      " [ 0.18709805  4.66017383  1.74591419]\n",
      " [ 0.03635545  5.26370734  1.16520236]]\n",
      "sqP = \n",
      "[[1.5065297667048299, 0.0, 0.0], [-0.012603843124549725, 0.5339604013351897, 0.0], [0.017148527604654747, -0.011134045864973985, 0.5307439114540182]]\n",
      "m = \n",
      "[0.08828605 5.1098433  1.72862158]\n",
      "P = \n",
      "[[ 2.26931071 -0.01401407  0.01942838]\n",
      " [-0.01401407  0.07577752 -0.00525057]\n",
      " [ 0.01942838 -0.00525057  0.08142038]]\n",
      "num = \n",
      "1347\n",
      "SENSOR 8 1.10085634846 0.0128641004662\n",
      "\n",
      "num = \n",
      "1348\n",
      "SENSOR 1 3.15319314656 1.83602899903\n",
      "\n",
      "num = \n",
      "1349\n",
      "SENSOR 4 3.96197077783 -1.56149331217\n",
      "\n",
      "num = \n",
      "1350\n",
      "SENSOR 7 3.16144997508 0.0497659131117\n",
      "\n",
      "num = \n",
      "1351\n",
      "ODOMETRY 0.000216898596734 0.100008870944 8.44491124946e-05\n",
      "\n",
      "sqP = \n",
      "[[1.5064231508504466, 0.0, 0.0], [-0.009302880494448702, 0.2751199241697126, 0.0], [0.012897030148667471, -0.018648567542444663, 0.28444029337421983]]\n",
      "sigma = \n",
      "[[ 0.08828605  5.1098433   1.72862158]\n",
      " [ 3.10113235  5.1098433   1.72862158]\n",
      " [ 0.06968028  5.66008315  1.72862158]\n",
      " [ 0.11408011  5.07254617  2.29750217]\n",
      " [-2.92456026  5.1098433   1.72862158]\n",
      " [ 0.10689181  4.55960346  1.72862158]\n",
      " [ 0.06249199  5.14714044  1.159741  ]]\n",
      "sigmax = \n",
      "[[ 0.08823391  5.2095848   1.73745708]\n",
      " [ 3.10108021  5.00979291  1.73266512]\n",
      " [ 0.06962815  5.75997177  1.73559989]\n",
      " [ 0.11402797  5.17202661  2.30890718]\n",
      " [-2.92461239  5.0120617   1.7070533 ]\n",
      " [ 0.10683967  4.65916331  1.73931121]\n",
      " [ 0.06243985  5.24707663  1.1660011 ]]\n",
      "sqP = \n",
      "[[1.5714845167517042, 0.0, 0.0], [-0.002326841177113869, 0.5324298460504172, 0.0], [0.00848828781471009, -0.00928720994683697, 0.5306423047952469]]\n",
      "m = \n",
      "[0.0908947  5.06243707 1.87343667]\n",
      "P = \n",
      "[[ 2.4695088  -0.00275209  0.01014593]\n",
      " [-0.00275209  0.07564078 -0.00520512]\n",
      " [ 0.01014593 -0.00520512  0.08108742]]\n",
      "num = \n",
      "1352\n",
      "SENSOR 8 0.98718385021 0.0505647532356\n",
      "\n",
      "num = \n",
      "1353\n",
      "SENSOR 1 3.44793567727 1.89256285855\n",
      "\n",
      "num = \n",
      "1354\n",
      "SENSOR 4 3.92237270173 -1.60631882591\n",
      "\n",
      "num = \n",
      "1355\n",
      "SENSOR 7 3.21973581934 -0.0340907108888\n",
      "\n",
      "num = \n",
      "1356\n",
      "ODOMETRY 0.000128009797301 0.100043460223 0.000987839174826\n",
      "\n",
      "sqP = \n",
      "[[1.571467084342163, 0.0, 0.0], [-0.0017512865633198449, 0.27502311019671843, 0.0], [0.006456342296587675, -0.018885010640407568, 0.2840582481793943]]\n",
      "sigma = \n",
      "[[ 0.0908947   5.06243707  1.87343667]\n",
      " [ 3.23382887  5.06243707  1.87343667]\n",
      " [ 0.08739212  5.61248329  1.87343667]\n",
      " [ 0.10380738  5.02466705  2.44155317]\n",
      " [-3.05203947  5.06243707  1.87343667]\n",
      " [ 0.09439727  4.51239085  1.87343667]\n",
      " [ 0.07798201  5.10020709  1.30532018]]\n",
      "sigmax = \n",
      "[[ 0.09119604  5.16203112  1.88253604]\n",
      " [-3.04905509  4.96285531  1.86420371]\n",
      " [ 0.08769347  5.7121086   1.88218715]\n",
      " [ 0.10410873  5.1241353   2.45193777]\n",
      " [-3.05173812  4.96283089  1.86447092]\n",
      " [ 0.09469862  4.61195242  1.88288482]\n",
      " [ 0.07828336  5.19991033  1.31313279]]\n",
      "sqP = \n",
      "[[1.4319902326763336, 0.0, 0.0], [0.08151291869788796, 0.5261178480151696, 0.0], [0.00876972650555317, -0.010409204306202891, 0.530029876803947]]\n",
      "m = \n",
      "[-0.68468149  5.16452953  1.95037604]\n",
      "P = \n",
      "[[ 2.04569452  0.08560137  0.0085087 ]\n",
      " [ 0.08560137  0.08267145 -0.00568197]\n",
      " [ 0.0085087  -0.00568197  0.08166918]]\n",
      "num = \n",
      "1357\n",
      "SENSOR 8 0.89829332316 0.135383956612\n",
      "\n",
      "num = \n",
      "1358\n",
      "SENSOR 1 3.27245387945 1.88010743562\n",
      "\n",
      "num = \n",
      "1359\n",
      "SENSOR 4 3.96502932983 -1.56994091616\n",
      "\n",
      "num = \n",
      "1360\n",
      "SENSOR 7 2.97710577284 0.0715555315754\n",
      "\n",
      "num = \n",
      "1361\n",
      "ODOMETRY -0.000150318169053 0.100071664514 0.000144679939358\n",
      "\n",
      "sqP = \n",
      "[[1.4302777781465414, 0.0, 0.0], [0.05984946908573281, 0.28122854148112, 0.0], [0.005948985427106742, -0.021470142528958583, 0.28490845375844825]]\n",
      "sigma = \n",
      "[[-0.68468149  5.16452953  1.95037604]\n",
      " [ 2.17587406  5.16452953  1.95037604]\n",
      " [-0.56498256  5.72698661  1.95037604]\n",
      " [-0.67278352  5.12158924  2.52019294]\n",
      " [-3.54523705  5.16452953  1.95037604]\n",
      " [-0.80438043  4.60207244  1.95037604]\n",
      " [-0.69657947  5.20746981  1.38055913]]\n",
      "sigmax = \n",
      "[[-0.68356565  5.24203334  1.88711584]\n",
      " [ 2.17698991  5.10761167  2.03265028]\n",
      " [-0.56386671  5.81148997  1.89682348]\n",
      " [-0.67166767  5.19984022  2.45785934]\n",
      " [ 2.73906411  5.07252096  1.98965858]\n",
      " [-0.80326458  4.67146758  1.8783135 ]\n",
      " [-0.69546362  5.28421549  1.31638129]]\n",
      "sqP = \n",
      "[[1.4401088976518237, 0.0, 0.0], [-0.051181391911275055, 0.532400322277039, 0.0], [0.05099568004603439, -0.01037198662264555, 0.5307940256623788]]\n",
      "m = \n",
      "[0.12720964 5.09228692 2.07373749]\n",
      "P = \n",
      "[[ 2.0702206  -0.05364651  0.05470623]\n",
      " [-0.05364651  0.07854215 -0.00696463]\n",
      " [ 0.05470623 -0.00696463  0.08676795]]\n",
      "num = \n",
      "1362\n",
      "SENSOR 8 0.774339649168 -0.000516516086974\n",
      "\n",
      "num = \n",
      "1363\n",
      "SENSOR 1 3.48116649203 1.98621024178\n",
      "\n",
      "num = \n",
      "1364\n",
      "SENSOR 4 3.85294043277 -1.65568756419\n",
      "\n",
      "num = \n",
      "1365\n",
      "SENSOR 7 2.90643136834 0.027023615806\n",
      "\n",
      "num = \n",
      "1366\n",
      "ODOMETRY -0.000465690711615 0.100017674194 -0.000142742436314\n",
      "\n",
      "sqP = \n",
      "[[1.4388261179601909, 0.0, 0.0], [-0.037284914851426855, 0.2777624589942119, 0.0], [0.03802143537144321, -0.019970297798883076, 0.2914163778621366]]\n",
      "sigma = \n",
      "[[ 0.12720964  5.09228692  2.07373749]\n",
      " [ 3.00486187  5.09228692  2.07373749]\n",
      " [ 0.05263981  5.64781184  2.07373749]\n",
      " [ 0.20325251  5.05234632  2.65657025]\n",
      " [-2.7504426   5.09228692  2.07373749]\n",
      " [ 0.20177947  4.536762    2.07373749]\n",
      " [ 0.05116677  5.13222751  1.49090473]]\n",
      "sigmax = \n",
      "[[ 0.127204    5.19155189  2.08641834]\n",
      " [ 3.00485624  4.99315128  2.08739267]\n",
      " [ 0.05263417  5.74774568  2.07898779]\n",
      " [ 0.20324687  5.15036107  2.67675557]\n",
      " [-2.75044824  4.99976781  2.03559889]\n",
      " [ 0.20177383  4.63480637  2.09377842]\n",
      " [ 0.05116113  5.23216898  1.49600782]]\n",
      "sqP = \n",
      "[[1.5076658755001309, 0.0, 0.0], [-0.008975187198589328, 0.5337394701229844, 0.0], [0.01989290949947088, -0.011239448385243999, 0.5356405971623809]]\n",
      "m = \n",
      "[0.13134126 5.18054226 2.209966  ]\n",
      "P = \n",
      "[[ 2.27273089 -0.01006115  0.02272974]\n",
      " [-0.01006115  0.07753852 -0.00665247]\n",
      " [ 0.02272974 -0.00665247  0.08979878]]\n",
      "num = \n",
      "1367\n",
      "SENSOR 8 0.527252645966 0.172607270039\n",
      "\n",
      "num = \n",
      "1368\n",
      "SENSOR 1 3.3814040867 1.97634147246\n",
      "\n",
      "num = \n",
      "1369\n",
      "SENSOR 4 4.10861734812 -1.59369624478\n",
      "\n",
      "num = \n",
      "1370\n",
      "SENSOR 7 2.76353072157 0.00840562337829\n",
      "\n",
      "num = \n",
      "1371\n",
      "ODOMETRY -0.000128208315622 0.0999656883624 0.000594809485977\n",
      "\n",
      "sqP = \n",
      "[[1.5075579233890712, 0.0, 0.0], [-0.006673808142594695, 0.2783774050591217, 0.0], [0.015077191836096635, -0.023535851022847533, 0.29835804666318827]]\n",
      "sigma = \n",
      "[[ 0.13134126  5.18054226  2.209966  ]\n",
      " [ 3.14645711  5.18054226  2.209966  ]\n",
      " [ 0.11799365  5.73729707  2.209966  ]\n",
      " [ 0.16149565  5.13347055  2.80668209]\n",
      " [-2.88377458  5.18054226  2.209966  ]\n",
      " [ 0.14468888  4.62378745  2.209966  ]\n",
      " [ 0.10118688  5.22761396  1.61324991]]\n",
      "sigmax = \n",
      "[[ 0.13073283  5.27970458  2.22301853]\n",
      " [-3.13733663  5.08052555  2.20952605]\n",
      " [ 0.11738521  5.83662477  2.22169383]\n",
      " [ 0.16088721  5.23219427  2.82271842]\n",
      " [-2.88438302  5.08381844  2.1845094 ]\n",
      " [ 0.14408045  4.72276672  2.22434091]\n",
      " [ 0.10057845  5.32712473  1.62330678]]\n",
      "sqP = \n",
      "[[1.4334649567737465, 0.0, 0.0], [0.07965421848513331, 0.5282767831501141, 0.0], [0.01356447640355462, -0.014052035215929806, 0.5382347071096649]]\n",
      "m = \n",
      "[-0.67841501  5.04254999  2.37303539]\n",
      "P = \n",
      "[[ 2.05008493  0.08377244  0.014237  ]\n",
      " [ 0.08377244  0.08316957 -0.00336925]\n",
      " [ 0.014237   -0.00336925  0.09598429]]\n",
      "num = \n",
      "1372\n",
      "SENSOR 8 0.687305913164 0.0798724661809\n",
      "\n",
      "num = \n",
      "1373\n",
      "SENSOR 1 3.21764284755 1.9744178959\n",
      "\n",
      "num = \n",
      "1374\n",
      "SENSOR 4 4.08919040605 -1.62141915547\n",
      "\n",
      "num = \n",
      "1375\n",
      "SENSOR 7 2.55473025478 0.0977268515407\n",
      "\n",
      "num = \n",
      "1376\n",
      "ODOMETRY 0.000821476656511 0.0999158037258 -0.00068566679204\n",
      "\n",
      "sqP = \n",
      "[[1.4318117658874512, 0.0, 0.0], [0.05850799585079697, 0.2823940203658031, 0.0], [0.009943348811545667, -0.013991141854215624, 0.30933746129541984]]\n",
      "sigma = \n",
      "[[-0.67841501  5.04254999  2.37303539]\n",
      " [ 2.18520852  5.04254999  2.37303539]\n",
      " [-0.56139902  5.60733803  2.37303539]\n",
      " [-0.65852831  5.01456771  2.99171032]\n",
      " [-3.54203854  5.04254999  2.37303539]\n",
      " [-0.795431    4.47776195  2.37303539]\n",
      " [-0.69830171  5.07053227  1.75436047]]\n",
      "sigmax = \n",
      "[[-0.67794841  5.12037207  2.31029097]\n",
      " [ 2.18567512  4.98493241  2.45472599]\n",
      " [-0.56093242  5.69195328  2.31980571]\n",
      " [-0.65806171  5.09362209  2.93052582]\n",
      " [ 2.74161336  4.95049786  2.41201672]\n",
      " [-0.7949644   4.54772648  2.30163439]\n",
      " [-0.69783511  5.14709127  1.69008093]]\n",
      "sqP = \n",
      "[[1.4399269388197984, 0.0, 0.0], [-0.05167457995631684, 0.5327007558570663, 0.0], [0.051585255544536586, -0.006763358281203308, 0.5444542855945161]]\n",
      "m = \n",
      "[0.12955697 4.95299262 2.44364964]\n",
      "P = \n",
      "[[ 2.06986303 -0.0551578   0.05654115]\n",
      " [-0.0551578   0.08267167 -0.01179266]\n",
      " [ 0.05654115 -0.01179266  0.10378354]]\n",
      "num = \n",
      "1377\n",
      "SENSOR 8 0.480616169488 0.139441754953\n",
      "\n",
      "num = \n",
      "1378\n",
      "SENSOR 1 3.45668051094 2.00189239553\n",
      "\n",
      "num = \n",
      "1379\n",
      "SENSOR 4 4.10524461182 -1.67981251381\n",
      "\n",
      "num = \n",
      "1380\n",
      "SENSOR 7 2.33739775251 0.0985219039275\n",
      "\n",
      "num = \n",
      "1381\n",
      "ODOMETRY 0.000455722461866 0.0999653812 -4.23758429747e-05\n",
      "\n",
      "sqP = \n",
      "[[1.4387018573035477, 0.0, 0.0], [-0.03833858930485273, 0.2849593359426584, 0.0], [0.03930011818385177, -0.03609618549317699, 0.3177044358039486]]\n",
      "sigma = \n",
      "[[ 0.12955697  4.95299262  2.44364964]\n",
      " [ 3.00696068  4.95299262  2.44364964]\n",
      " [ 0.05287979  5.52291129  2.44364964]\n",
      " [ 0.2081572   4.88080025  3.07905852]\n",
      " [-2.74784675  4.95299262  2.44364964]\n",
      " [ 0.20623415  4.38307395  2.44364964]\n",
      " [ 0.05095673  5.02518499  1.80824077]]\n",
      "sigmax = \n",
      "[[ 0.12969278  5.05206041  2.45663964]\n",
      " [ 3.00709649  4.85396999  2.45697956]\n",
      " [ 0.0530156   5.62268306  2.44901267]\n",
      " [ 0.20829301  4.97854221  3.09978714]\n",
      " [-2.74771094  4.86075405  2.40524112]\n",
      " [ 0.20636996  4.48085559  2.46419027]\n",
      " [ 0.05109254  5.12496689  1.81341192]]\n",
      "sqP = \n",
      "[[1.5076065073247158, 0.0, 0.0], [-0.009831942458023818, 0.538353960735819, 0.0], [0.020823201274030298, -0.021812024737261095, 0.5502879783329727]]\n",
      "m = \n",
      "[0.13769522 4.98754377 2.65811863]\n",
      "P = \n",
      "[[ 2.27253803 -0.01106295  0.02410021]\n",
      " [-0.01106295  0.08218752 -0.01286589]\n",
      " [ 0.02410021 -0.01286589  0.10678597]]\n",
      "num = \n",
      "1382\n",
      "SENSOR 8 0.400741221794 0.167751600491\n",
      "\n",
      "num = \n",
      "1383\n",
      "SENSOR 1 3.55108397687 2.05110783316\n",
      "\n",
      "num = \n",
      "1384\n",
      "SENSOR 4 4.06515935986 -1.72057455144\n",
      "\n",
      "num = \n",
      "1385\n",
      "SENSOR 7 2.50287051778 0.127838872546\n",
      "\n",
      "num = \n",
      "1386\n",
      "ODOMETRY 0.000100633095148 0.100219327349 0.000631070639127\n",
      "\n",
      "sqP = \n",
      "[[1.5074939553585243, 0.0, 0.0], [-0.007338637844976484, 0.2865897130545126, 0.0], [0.015986933643435487, -0.044483685952609014, 0.32334438002237886]]\n",
      "sigma = \n",
      "[[ 0.13769522  4.98754377  2.65811863]\n",
      " [ 3.15268313  4.98754377  2.65811863]\n",
      " [ 0.12301794  5.56072319  2.65811863]\n",
      " [ 0.16966908  4.89857639  3.30480739]\n",
      " [-2.87729269  4.98754377  2.65811863]\n",
      " [ 0.15237249  4.41436434  2.65811863]\n",
      " [ 0.10572135  5.07651114  2.01142987]]\n",
      "sigmax = \n",
      "[[ 0.13810856  5.08655671  2.67188506]\n",
      " [-3.13008883  4.88758505  2.65696444]\n",
      " [ 0.12343129  5.65992752  2.67043039]\n",
      " [ 0.17008243  4.99709864  3.32173207]\n",
      " [-2.87687935  4.89106153  2.63196034]\n",
      " [ 0.15278584  4.51316457  2.67333676]\n",
      " [ 0.1061347   5.17591357  2.02202397]]\n",
      "sqP = \n",
      "[[1.4334808868926232, 0.0, 0.0], [0.07911125056527922, 0.534064227067808, 0.0], [0.014618837767295716, -0.027865170416430017, 0.5520579381723266]]\n",
      "m = \n",
      "[-0.64630968  5.0485994   2.64430187]\n",
      "P = \n",
      "[[ 2.05023965  0.08358309  0.01476138]\n",
      " [ 0.08358309  0.08944115 -0.01056751]\n",
      " [ 0.01476138 -0.01056751  0.10956572]]\n",
      "num = \n",
      "1387\n",
      "SENSOR 8 0.417382565095 0.188823271199\n",
      "\n",
      "num = \n",
      "1388\n",
      "SENSOR 1 3.35123641832 2.09116664517\n",
      "\n",
      "num = \n",
      "1389\n",
      "SENSOR 4 3.95754572184 -1.71268379099\n",
      "\n",
      "num = \n",
      "1390\n",
      "SENSOR 7 2.41072585607 0.122504676512\n",
      "\n",
      "num = \n",
      "1391\n",
      "ODOMETRY -0.000607763710215 0.100140453337 -0.000218697666427\n",
      "\n",
      "sqP = \n",
      "[[1.4318657921266407, 0.0, 0.0], [0.058373549241569886, 0.2933149796588841, 0.0], [0.010309190129972858, -0.038079527018605674, 0.32864782486214095]]\n",
      "sigma = \n",
      "[[-0.64630968  5.0485994   2.64430187]\n",
      " [ 2.21742191  5.0485994   2.64430187]\n",
      " [-0.52956258  5.63522936  2.64430187]\n",
      " [-0.6256913   4.97244034  3.30159752]\n",
      " [-3.51004126  5.0485994   2.64430187]\n",
      " [-0.76305678  4.46196944  2.64430187]\n",
      " [-0.66692806  5.12475845  1.98700622]]\n",
      "sigmax = \n",
      "[[-0.64557797  5.12861173  2.58395338]\n",
      " [ 2.21815361  4.98820954  2.72428298]\n",
      " [-0.52883087  5.72172655  2.59368419]\n",
      " [-0.62495959  5.05367987  3.24291147]\n",
      " [ 2.77387575  4.95510243  2.68038831]\n",
      " [-0.76232507  4.5344076   2.57504419]\n",
      " [-0.66619635  5.20350958  1.92502095]]\n",
      "sqP = \n",
      "[[1.4399188412267048, 0.0, 0.0], [-0.05316503120934249, 0.539778708329477, 0.0], [0.04985959474848943, -0.021660694778818688, 0.555302296125942]]\n",
      "m = \n",
      "[0.1462888  5.03300051 2.62447676]\n",
      "P = \n",
      "[[ 2.06984535 -0.0558898   0.05450753]\n",
      " [-0.0558898   0.08565417 -0.01289749]\n",
      " [ 0.05450753 -0.01289749  0.11184352]]\n",
      "num = \n",
      "1392\n",
      "SENSOR 8 0.369346735302 0.271181092817\n",
      "\n",
      "num = \n",
      "1393\n",
      "SENSOR 1 3.67699620635 2.07721447415\n",
      "\n",
      "num = \n",
      "1394\n",
      "SENSOR 4 4.07710876539 -1.73456190265\n",
      "\n",
      "num = \n",
      "1395\n",
      "SENSOR 7 2.42576722134 -0.0138112591739\n",
      "\n",
      "num = \n",
      "1396\n",
      "ODOMETRY -0.000139407170528 0.0998511677938 0.000334065170192\n",
      "\n",
      "sqP = \n",
      "[[1.4386957097797837, 0.0, 0.0], [-0.03884754700734504, 0.29007764164296623, 0.0], [0.037886765164855236, -0.0393883751244309, 0.32993434336812866]]\n",
      "sigma = \n",
      "[[ 0.1462888   5.03300051  2.62447676]\n",
      " [ 3.02368022  5.03300051  2.62447676]\n",
      " [ 0.06859371  5.61315579  2.62447676]\n",
      " [ 0.22206233  4.95422376  3.28434545]\n",
      " [-2.73110262  5.03300051  2.62447676]\n",
      " [ 0.22398389  4.45284523  2.62447676]\n",
      " [ 0.07051527  5.11177726  1.96460808]]\n",
      "sigmax = \n",
      "[[ 0.14546234  5.1320802   2.63901378]\n",
      " [ 3.02285376  4.93356257  2.63631766]\n",
      " [ 0.06776725  5.71306491  2.63127966]\n",
      " [ 0.22123587  5.05191868  3.30634119]\n",
      " [-2.73192908  4.94115492  2.58457066]\n",
      " [ 0.22315743  4.5504977   2.64666019]\n",
      " [ 0.06968881  5.21167312  1.97160295]]\n",
      "sqP = \n",
      "[[1.5075774886293243, 0.0, 0.0], [-0.010304368506243866, 0.5413504701326053, 0.0], [0.020830538689005557, -0.024408139182129685, 0.5573603853473511]]\n",
      "m = \n",
      "[0.14797307 5.09210531 2.70517262]\n",
      "P = \n",
      "[[ 2.27244145 -0.0114422   0.02399975]\n",
      " [-0.0114422   0.08324329 -0.01078332]\n",
      " [ 0.02399975 -0.01078332  0.11043321]]\n",
      "num = \n",
      "1397\n",
      "SENSOR 8 0.335693906267 0.462318775119\n",
      "\n",
      "num = \n",
      "1398\n",
      "SENSOR 1 3.54703844972 2.08539235736\n",
      "\n",
      "num = \n",
      "1399\n",
      "SENSOR 4 3.94824350404 -1.6777048794\n",
      "\n",
      "num = \n",
      "1400\n",
      "SENSOR 7 2.13297568426 0.0826896741318\n",
      "\n",
      "num = \n",
      "1401\n",
      "ODOMETRY -0.000628418701319 0.0999704949615 -0.000124838470818\n",
      "\n",
      "sqP = \n",
      "[[1.5074619225624741, 0.0, 0.0], [-0.007590374709993185, 0.2884192743643137, 0.0], [0.015920631864108793, -0.03696865513436464, 0.3298682565109686]]\n",
      "sigma = \n",
      "[[ 0.14797307  5.09210531  2.70517262]\n",
      " [ 3.16289691  5.09210531  2.70517262]\n",
      " [ 0.13279232  5.66894386  2.70517262]\n",
      " [ 0.17981433  5.018168    3.36490913]\n",
      " [-2.86695078  5.09210531  2.70517262]\n",
      " [ 0.16315382  4.51526676  2.70517262]\n",
      " [ 0.1161318   5.16604262  2.0454361 ]]\n",
      "sigmax = \n",
      "[[ 0.14816773  5.19086735  2.71988027]\n",
      " [-3.12009374  4.99227651  2.70305944]\n",
      " [ 0.13298698  5.76791779  2.71837935]\n",
      " [ 0.18000899  5.11641175  3.38275351]\n",
      " [-2.86675612  4.99599255  2.67810615]\n",
      " [ 0.16334847  4.61379416  2.7213778 ]\n",
      " [ 0.11632646  5.26522283  2.05699213]]\n",
      "sqP = \n",
      "[[1.4334828747742059, 0.0, 0.0], [0.07893336200141375, 0.5344716739460432, 0.0], [0.015447111468867082, -0.023745674673101697, 0.556103062501785]]\n",
      "m = \n",
      "[-0.63992222  5.09611661  2.83095625]\n",
      "P = \n",
      "[[ 2.0502523   0.08329711  0.01592055]\n",
      " [ 0.08329711  0.08881873 -0.00742018]\n",
      " [ 0.01592055 -0.00742018  0.10992654]]\n",
      "num = \n",
      "1402\n",
      "SENSOR 8 0.0237407258792 0.999084065632\n",
      "\n",
      "num = \n",
      "1403\n",
      "SENSOR 1 3.49005275028 2.05858832887\n",
      "\n",
      "num = \n",
      "1404\n",
      "SENSOR 4 4.07307706178 -1.72646904791\n",
      "\n",
      "num = \n",
      "1405\n",
      "SENSOR 7 2.07921259872 0.00213504582166\n",
      "\n",
      "num = \n",
      "1406\n",
      "ODOMETRY -9.78009794874e-05 0.100003974246 -0.00134217319887\n",
      "\n",
      "sqP = \n",
      "[[1.4318702105903507, 0.0, 0.0], [0.0581736479853253, 0.29229190673228533, 0.0], [0.011118707139117407, -0.027599112753557767, 0.3302138691718827]]\n",
      "sigma = \n",
      "[[-0.63992222  5.09611661  2.83095625]\n",
      " [ 2.2238182   5.09611661  2.83095625]\n",
      " [-0.52357492  5.68070043  2.83095625]\n",
      " [-0.6176848   5.04091839  3.49138399]\n",
      " [-3.50366264  5.09611661  2.83095625]\n",
      " [-0.75626951  4.5115328   2.83095625]\n",
      " [-0.66215963  5.15131484  2.17052851]]\n",
      "sigmax = \n",
      "[[-0.64067547  5.17626964  2.77121018]\n",
      " [ 2.22306495  5.03542552  2.91039611]\n",
      " [-0.52432818  5.76724718  2.78091867]\n",
      " [-0.61843806  5.12238008  3.43343494]\n",
      " [ 2.77876941  5.00264992  2.86642562]\n",
      " [-0.75702277  4.58420831  2.76230955]\n",
      " [-0.66291289  5.23011956  2.10901497]]\n",
      "sqP = \n",
      "[[1.4399163482425983, 0.0, 0.0], [-0.05329431869371839, 0.5385633843666178, 0.0], [0.04957206449055991, -0.015332146652786684, 0.5564619752494964]]\n",
      "m = \n",
      "[0.16513412 5.0117303  2.89363355]\n",
      "P = \n",
      "[[ 2.06984993 -0.05639305  0.0540591 ]\n",
      " [-0.05639305  0.08708335 -0.01112699]\n",
      " [ 0.0540591  -0.01112699  0.11151447]]\n",
      "num = \n",
      "1407\n",
      "SENSOR 8 0.129747875719 2.20808064967\n",
      "\n",
      "num = \n",
      "1408\n",
      "SENSOR 1 3.82990788235 2.17088111054\n",
      "\n",
      "num = \n",
      "1409\n",
      "SENSOR 4 4.01274770551 -1.80022287123\n",
      "\n",
      "num = \n",
      "1410\n",
      "SENSOR 7 1.74885650734 -0.0121223549881\n",
      "\n",
      "num = \n",
      "1411\n",
      "ODOMETRY 0.000936885880489 0.0999064874362 0.000600172698976\n",
      "\n",
      "sqP = \n",
      "[[1.4386973026314716, 0.0, 0.0], [-0.03919730315747294, 0.29248404680975937, 0.0], [0.03757503335592636, -0.03300745606456784, 0.3301713049850708]]\n",
      "sigma = \n",
      "[[ 0.16513412  5.0117303   2.89363355]\n",
      " [ 3.04252873  5.0117303   2.89363355]\n",
      " [ 0.08673951  5.5966984   2.89363355]\n",
      " [ 0.24028419  4.94571539  3.55397616]\n",
      " [-2.71226048  5.0117303   2.89363355]\n",
      " [ 0.24352873  4.42676221  2.89363355]\n",
      " [ 0.08998405  5.07774521  2.23329094]]\n",
      "sigmax = \n",
      "[[ 0.16369415  5.11037546  2.91006302]\n",
      " [ 3.04108875  4.9122176   2.90353387]\n",
      " [ 0.08529954  5.69632725  2.90228723]\n",
      " [ 0.23884421  5.04284862  3.57776547]\n",
      " [-2.71370046  4.92079822  2.85201442]\n",
      " [ 0.24208875  4.52381774  2.91773789]\n",
      " [ 0.08854408  5.17734547  2.24226782]]\n",
      "sqP = \n",
      "[[1.5075802599880346, 0.0, 0.0], [-0.01050654810216279, 0.5422006192793101, 0.0], [0.020713306741716224, -0.020282452548853424, 0.5576805601138165]]\n",
      "m = \n",
      "[0.17136727 5.12285501 3.14504094]\n",
      "P = \n",
      "[[ 2.27244967 -0.01179685  0.02381057]\n",
      " [-0.01179685  0.08664569 -0.01058888]\n",
      " [ 0.02381057 -0.01058888  0.10864134]]\n",
      "num = \n",
      "1412\n",
      "SENSOR 8 0.00599324163092 2.67223936752\n",
      "\n",
      "num = \n",
      "1413\n",
      "SENSOR 1 3.82421795548 2.19552984414\n",
      "\n",
      "num = \n",
      "1414\n",
      "SENSOR 3 4.79851180987 0.60594351723\n",
      "\n",
      "num = \n",
      "1415\n",
      "SENSOR 4 4.05133471121 -1.81575940442\n",
      "\n",
      "num = \n",
      "1416\n",
      "SENSOR 7 1.85931144483 0.164227994673\n",
      "\n",
      "num = \n",
      "1417\n",
      "ODOMETRY -0.000253202498002 0.10008722051 0.000209063753791\n",
      "\n",
      "sqP = \n",
      "[[1.5074646502699685, 0.0, 0.0], [-0.00782562192337879, 0.2942523631619288, 0.0], [0.01579511301570237, -0.03556565366693849, 0.3273025138992566]]\n",
      "sigma = \n",
      "[[ 0.17136727  5.12285501  3.14504094]\n",
      " [ 3.18629657  5.12285501  3.14504094]\n",
      " [ 0.15571602  5.71135973  3.14504094]\n",
      " [ 0.20295749  5.0517237   3.79964596]\n",
      " [-2.84356203  5.12285501  3.14504094]\n",
      " [ 0.18701851  4.53435028  3.14504094]\n",
      " [ 0.13977704  5.19398631  2.49043591]]\n",
      "sigmax = \n",
      "[[ 0.17290433  5.22128211  3.16217019]\n",
      " [-3.09535168  5.02305256  3.14048271]\n",
      " [ 0.15725308  5.81004287  3.16062765]\n",
      " [ 0.20449455  5.14956067  3.81987549]\n",
      " [-2.84202497  5.02738026  3.11561511]\n",
      " [ 0.18855557  4.63249725  3.16370853]\n",
      " [ 0.1413141   5.29290534  2.5044478 ]]\n",
      "sqP = \n",
      "[[1.4334826306619064, 0.0, 0.0], [0.07860503310725703, 0.5375651930946321, 0.0], [0.01737281916977998, -0.022564547328951017, 0.5546110336241342]]\n",
      "m = \n",
      "[-0.62338987  5.08854236  3.18969783]\n",
      "P = \n",
      "[[2.05022863 0.08227282 0.01959921]\n",
      " [0.08227282 0.08167052 0.01097972]\n",
      " [0.01959921 0.01097972 0.08079143]]\n",
      "num = \n",
      "1418\n",
      "SENSOR 8 0.355098876696 2.89500927945\n",
      "\n",
      "num = \n",
      "1419\n",
      "SENSOR 1 3.90659962847 2.1333112384\n",
      "\n",
      "num = \n",
      "1420\n",
      "SENSOR 3 4.81068150996 0.62679112096\n",
      "\n",
      "num = \n",
      "1421\n",
      "SENSOR 4 4.09111006731 -1.88931307412\n",
      "\n",
      "num = \n",
      "1422\n",
      "SENSOR 7 1.79380510621 -0.0357020447254\n",
      "\n",
      "num = \n",
      "1423\n",
      "ODOMETRY 0.000157129099918 0.100032988722 -0.000729862285518\n",
      "\n",
      "sqP = \n",
      "[[1.4318619438244944, 0.0, 0.0], [0.057458626107487525, 0.2799446923776794, 0.0], [0.013687916283902328, 0.03641160474342067, 0.2815639695926805]]\n",
      "sigma = \n",
      "[[-0.62338987  5.08854236  3.18969783]\n",
      " [ 2.24033402  5.08854236  3.18969783]\n",
      " [-0.50847262  5.64843174  3.18969783]\n",
      " [-0.59601404  5.16136557  3.75282577]\n",
      " [-3.48711376  5.08854236  3.18969783]\n",
      " [-0.73830712  4.52865298  3.18969783]\n",
      " [-0.6507657   5.01571915  2.62656989]]\n",
      "sigmax = \n",
      "[[-0.62343401  5.16978879  3.13124726]\n",
      " [ 2.24028988  5.02644575  3.26819281]\n",
      " [-0.50851676  5.7358445   3.14094887]\n",
      " [-0.59605818  5.24418149  3.69662101]\n",
      " [ 2.79602741  4.99437897  3.22361992]\n",
      " [-0.73835126  4.60266132  3.12231671]\n",
      " [-0.65080984  5.09533521  2.56591731]]\n",
      "sqP = \n",
      "[[1.4399105669248962, 0.0, 0.0], [-0.05423665308334909, 0.5324615382330351, 0.0], [0.04864781376928529, 0.020936642775756804, 0.5288721870480836]]\n",
      "m = \n",
      "[0.172706   5.11028185 3.25117962]\n",
      "P = \n",
      "[[ 2.06934399 -0.05705117  0.05073072]\n",
      " [-0.05705117  0.07885194  0.00744792]\n",
      " [ 0.05073072  0.00744792  0.08019217]]\n",
      "num = \n",
      "1424\n",
      "SENSOR 8 0.310596091371 2.80972354331\n",
      "\n",
      "num = \n",
      "1425\n",
      "SENSOR 1 3.70515903504 2.23033190653\n",
      "\n",
      "num = \n",
      "1426\n",
      "SENSOR 3 4.65126987644 0.73166186817\n",
      "\n",
      "num = \n",
      "1427\n",
      "SENSOR 4 4.23935852045 -1.93054147639\n",
      "\n",
      "num = \n",
      "1428\n",
      "SENSOR 7 1.76174667646 0.0262704139452\n",
      "\n",
      "num = \n",
      "1429\n",
      "ODOMETRY -0.000548408308394 0.0999919238562 -0.000250033911117\n",
      "\n",
      "sqP = \n",
      "[[1.4385214603241439, 0.0, 0.0], [-0.039659586492278756, 0.2779911029295476, 0.0], [0.0352658767472718, 0.03182314051779284, 0.2791697916554406]]\n",
      "sigma = \n",
      "[[ 0.172706    5.11028185  3.25117962]\n",
      " [ 3.04974892  5.11028185  3.25117962]\n",
      " [ 0.09338683  5.66626405  3.25117962]\n",
      " [ 0.24323776  5.17392813  3.80951921]\n",
      " [-2.70433692  5.11028185  3.25117962]\n",
      " [ 0.25202517  4.55429964  3.25117962]\n",
      " [ 0.10217425  5.04663557  2.69284004]]\n",
      "sigmax = \n",
      "[[ 0.17213327  5.20882398  3.26838565]\n",
      " [ 3.04917619  5.01066903  3.26033846]\n",
      " [ 0.0928141   5.76585969  3.26052346]\n",
      " [ 0.24266502  5.27101269  3.83362704]\n",
      " [-2.70490965  5.01966691  3.20880592]\n",
      " [ 0.25145244  4.65116861  3.27613964]\n",
      " [ 0.10160152  5.14614526  2.7030587 ]]\n",
      "sqP = \n",
      "[[1.507368752719984, 0.0, 0.0], [-0.008748402115914471, 0.5343967321672073, 0.0], [0.019010028041383586, 0.017024835347423772, 0.5288010102315949]]\n",
      "m = \n",
      "[0.17587721 4.96352042 3.27424639]\n",
      "P = \n",
      "[[ 2.27183246 -0.00971482  0.02121871]\n",
      " [-0.00971482  0.07666929  0.01032983]\n",
      " [ 0.02121871  0.01032983  0.07703974]]\n",
      "num = \n",
      "1430\n",
      "SENSOR 8 0.672273832166 3.01287570212\n",
      "\n",
      "num = \n",
      "1431\n",
      "SENSOR 1 3.82284974892 2.26851849591\n",
      "\n",
      "num = \n",
      "1432\n",
      "SENSOR 3 4.66661981658 0.750783246804\n",
      "\n",
      "num = \n",
      "1433\n",
      "SENSOR 4 4.20611841782 -1.87591401282\n",
      "\n",
      "num = \n",
      "1434\n",
      "SENSOR 7 1.65083446213 -0.10214389997\n",
      "\n",
      "num = \n",
      "1435\n",
      "ODOMETRY -0.000371751330884 0.0999313796848 0.000860224474773\n",
      "\n",
      "sqP = \n",
      "[[1.5072599180371553, 0.0, 0.0], [-0.006445353238479217, 0.2768171747514279, 0.0], [0.01407766909160751, 0.037644206454483214, 0.27463516042810615]]\n",
      "sigma = \n",
      "[[ 0.17587721  4.96352042  3.27424639]\n",
      " [ 3.19039705  4.96352042  3.27424639]\n",
      " [ 0.1629865   5.51715477  3.27424639]\n",
      " [ 0.20403255  5.03880883  3.82351671]\n",
      " [-2.83864263  4.96352042  3.27424639]\n",
      " [ 0.18876792  4.40988607  3.27424639]\n",
      " [ 0.14772187  4.88823201  2.72497607]]\n",
      "sigmax = \n",
      "[[ 0.17507877  5.06197939  3.29168817]\n",
      " [-3.0935867   4.8636449   3.26942305]\n",
      " [ 0.16218806  5.61583039  3.29041755]\n",
      " [ 0.20323411  5.13673777  3.84372336]\n",
      " [-2.83944107  4.86806573  3.24446742]\n",
      " [ 0.18796947  4.50811203  3.29295589]\n",
      " [ 0.14692343  4.98714297  2.73963916]]\n",
      "sqP = \n",
      "[[1.4334669245530245, 0.0, 0.0], [0.07975708873710588, 0.5281473796142395, 0.0], [0.016712670462897074, 0.019094385549903562, 0.5251755206044559]]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "m = \n",
      "[-0.61048606  4.99653827  3.37071316]\n",
      "P = \n",
      "[[2.0500765  0.08382976 0.01844583]\n",
      " [0.08382976 0.08355785 0.01014215]\n",
      " [0.01844583 0.01014215 0.07447356]]\n",
      "num = \n",
      "1436\n",
      "SENSOR 8 0.556611108037 3.00403808456\n",
      "\n",
      "num = \n",
      "1437\n",
      "SENSOR 1 4.07609954779 2.29923731693\n",
      "\n",
      "num = \n",
      "1438\n",
      "SENSOR 3 4.61268174855 0.746177069223\n",
      "\n",
      "num = \n",
      "1439\n",
      "SENSOR 4 4.26511409215 -1.93342631232\n",
      "\n",
      "num = \n",
      "1440\n",
      "SENSOR 7 1.5749019735 0.109768246387\n",
      "\n",
      "num = \n",
      "1441\n",
      "ODOMETRY 2.93556084258e-05 0.100060189388 0.00098238378032\n",
      "\n",
      "sqP = \n",
      "[[1.4318088199140377, 0.0, 0.0], [0.05854815090004316, 0.28307235813688936, 0.0], [0.012882882715308555, 0.033164232433738255, 0.2705692641906409]]\n",
      "sigma = \n",
      "[[-0.61048606  4.99653827  3.37071316]\n",
      " [ 2.25313158  4.99653827  3.37071316]\n",
      " [-0.49338975  5.56268298  3.37071316]\n",
      " [-0.58472029  5.06286673  3.91185169]\n",
      " [-3.4741037   4.99653827  3.37071316]\n",
      " [-0.72758236  4.43039355  3.37071316]\n",
      " [-0.63625182  4.9302098   2.82957463]]\n",
      "sigmax = \n",
      "[[-0.60999758  5.07839769  3.31339548]\n",
      " [ 2.25362006  4.93354966  3.44829355]\n",
      " [-0.49290128  5.65067819  3.32335154]\n",
      " [-0.58423182  5.14617565  3.85666198]\n",
      " [ 2.80957008  4.90209269  3.40336763]\n",
      " [-0.72709388  4.50499604  3.30422445]\n",
      " [-0.63576335  5.01056538  2.77016704]]\n",
      "sqP = \n",
      "[[1.4399521543836384, 0.0, 0.0], [-0.05441767256072966, 0.5339565384899769, 0.0], [0.04747877482714163, 0.018490952141768893, 0.5231604613938421]]\n",
      "m = \n",
      "[0.18639781 5.05737185 3.47896609]\n",
      "P = \n",
      "[[ 2.06953286 -0.05729873  0.04947035]\n",
      " [-0.05729873  0.08096079  0.0067883 ]\n",
      " [ 0.04947035  0.0067883   0.07547978]]\n",
      "num = \n",
      "1442\n",
      "SENSOR 8 0.731212089539 3.13442910945\n",
      "\n",
      "num = \n",
      "1443\n",
      "SENSOR 1 4.19104478917 2.19441500315\n",
      "\n",
      "num = \n",
      "1444\n",
      "SENSOR 3 4.50331304373 0.707514185573\n",
      "\n",
      "num = \n",
      "1445\n",
      "SENSOR 4 4.34762941195 -1.97920192321\n",
      "\n",
      "num = \n",
      "1446\n",
      "SENSOR 7 1.33294282082 0.0230575892315\n",
      "\n",
      "num = \n",
      "1447\n",
      "ODOMETRY 1.97994777393e-05 0.099913771488 0.000324267521873\n",
      "\n",
      "sqP = \n",
      "[[1.438587105433246, 0.0, 0.0], [-0.039829862689625566, 0.2817345787776813, 0.0], [0.03438815278797335, 0.028956233062974533, 0.2710327792901829]]\n",
      "sigma = \n",
      "[[ 0.18639781  5.05737185  3.47896609]\n",
      " [ 3.06357202  5.05737185  3.47896609]\n",
      " [ 0.10673808  5.62084101  3.47896609]\n",
      " [ 0.25517411  5.11528432  4.02103165]\n",
      " [-2.6907764   5.05737185  3.47896609]\n",
      " [ 0.26605753  4.49390269  3.47896609]\n",
      " [ 0.1176215   4.99945938  2.93690053]]\n",
      "sigmax = \n",
      "[[ 0.18740955  5.15569827  3.49751216]\n",
      " [ 3.06458376  4.95761582  3.48676201]\n",
      " [ 0.10774982  5.72033143  3.48962898]\n",
      " [ 0.25618585  5.21210375  4.04629108]\n",
      " [-2.68976466  4.96730978  3.43536719]\n",
      " [ 0.26706927  4.59044149  3.50527773]\n",
      " [ 0.11863324  5.09882787  2.94864556]]\n",
      "sqP = \n",
      "[[1.5074156105510015, 0.0, 0.0], [-0.009130463578078453, 0.5361933324829506, 0.0], [0.018625456230322537, 0.015149316346308768, 0.5245514295162245]]\n",
      "m = \n",
      "[0.19452658 5.07373053 3.67021746]\n",
      "P = \n",
      "[[ 2.27197372 -0.01013967  0.0206761 ]\n",
      " [-0.01013967  0.07887874  0.00989162]\n",
      " [ 0.0206761   0.00989162  0.07108191]]\n",
      "num = \n",
      "1448\n",
      "SENSOR 8 0.726506378949 3.06634580351\n",
      "\n",
      "num = \n",
      "1449\n",
      "SENSOR 1 4.04114278918 2.33975587408\n",
      "\n",
      "num = \n",
      "1450\n",
      "SENSOR 3 4.50535713636 0.78187843772\n",
      "\n",
      "num = \n",
      "1451\n",
      "SENSOR 4 4.29965089498 -1.9407687623\n",
      "\n",
      "num = \n",
      "1452\n",
      "SENSOR 7 1.27944364032 0.0552717851415\n",
      "\n",
      "num = \n",
      "1453\n",
      "ODOMETRY -0.000438875804674 0.099903540815 0.000489960444763\n",
      "\n",
      "sqP = \n",
      "[[1.5073067755922742, 0.0, 0.0], [-0.00672701087147604, 0.28077301845643143, 0.0], [0.013717250233295949, 0.035558605337154804, 0.2638737097142418]]\n",
      "sigma = \n",
      "[[ 0.19452658  5.07373053  3.67021746]\n",
      " [ 3.20914013  5.07373053  3.67021746]\n",
      " [ 0.18107256  5.63527657  3.67021746]\n",
      " [ 0.22196108  5.14484774  4.19796488]\n",
      " [-2.82008697  5.07373053  3.67021746]\n",
      " [ 0.2079806   4.51218449  3.67021746]\n",
      " [ 0.16709208  5.00261332  3.14247004]]\n",
      "sigmax = \n",
      "[[ 0.19487064  5.17175948  3.68953294]\n",
      " [-3.07370111  4.97404474  3.66347169]\n",
      " [ 0.18141662  5.7335565   3.68821235]\n",
      " [ 0.22230514  5.24230995  4.21996213]\n",
      " [-2.81974291  4.9789369   3.63864329]\n",
      " [ 0.20832467  4.6099447   3.69085003]\n",
      " [ 0.16743614  5.10113522  3.15908921]]\n",
      "sqP = \n",
      "[[1.4334626482561006, 0.0, 0.0], [0.07930858430043092, 0.5301056361513259, 0.0], [0.018075500198207972, 0.017246821818919292, 0.5196470141763923]]\n",
      "m = \n",
      "[-0.60160597  5.04909121  3.67885974]\n",
      "P = \n",
      "[[2.05012481 0.08353027 0.01991437]\n",
      " [0.08353027 0.08557327 0.0114076 ]\n",
      " [0.01991437 0.0114076  0.06539787]]\n",
      "num = \n",
      "1454\n",
      "SENSOR 8 0.62179737963 3.10384573817\n",
      "\n",
      "num = \n",
      "1455\n",
      "SENSOR 1 4.24151843231 2.38316523462\n",
      "\n",
      "num = \n",
      "1456\n",
      "SENSOR 3 4.26416968603 0.789449674711\n",
      "\n",
      "num = \n",
      "1457\n",
      "SENSOR 4 4.33240098534 -1.96547159788\n",
      "\n",
      "num = \n",
      "1458\n",
      "SENSOR 7 1.24198454896 -0.0312764055356\n",
      "\n",
      "num = \n",
      "1459\n",
      "ODOMETRY 0.00124083409796 0.09994562889 -0.000946313040158\n",
      "\n",
      "sqP = \n",
      "[[1.4318256920169399, 0.0, 0.0], [0.0583382969345008, 0.286652952614671, 0.0], [0.013908373932269824, 0.03696528088955043, 0.2526618160547377]]\n",
      "sigma = \n",
      "[[-0.60160597  5.04909121  3.67885974]\n",
      " [ 2.26204541  5.04909121  3.67885974]\n",
      " [-0.48492938  5.62239712  3.67885974]\n",
      " [-0.57378922  5.12302177  4.18418337]\n",
      " [-3.46525735  5.04909121  3.67885974]\n",
      " [-0.71828256  4.47578531  3.67885974]\n",
      " [-0.62942272  4.97516065  3.17353611]]\n",
      "sigmax = \n",
      "[[-0.60155489  5.13142964  3.62228147]\n",
      " [ 2.2620965   4.98543649  3.7558584 ]\n",
      " [-0.48487829  5.71076212  3.63225133]\n",
      " [-0.57373814  5.20690197  4.12991708]\n",
      " [ 2.81797904  4.95438899  3.71067494]\n",
      " [-0.71823148  4.55097753  3.61308096]\n",
      " [-0.62937163  5.05589361  3.11468963]]\n",
      "sqP = \n",
      "[[1.4399515485909589, 0.0, 0.0], [-0.05462945727260017, 0.5361430921207314, 0.0], [0.046901361825078154, 0.0191520767895036, 0.5141069021503549]]\n",
      "m = \n",
      "[0.20008366 5.03762407 3.76390686]\n",
      "P = \n",
      "[[ 2.06950585 -0.05753011  0.04858026]\n",
      " [-0.05753011  0.08287735  0.00754847]\n",
      " [ 0.04858026  0.00754847  0.06722442]]\n",
      "num = \n",
      "1460\n",
      "SENSOR 8 0.760293224516 3.02148565162\n",
      "\n",
      "num = \n",
      "1461\n",
      "SENSOR 1 4.28994508486 2.2237346564\n",
      "\n",
      "num = \n",
      "1462\n",
      "SENSOR 3 4.26717238324 0.896293534553\n",
      "\n",
      "num = \n",
      "1463\n",
      "SENSOR 4 4.27865244716 -2.06179292551\n",
      "\n",
      "num = \n",
      "1464\n",
      "SENSOR 7 1.12266271613 0.0415462318364\n",
      "\n",
      "num = \n",
      "1465\n",
      "ODOMETRY 0.000129299270017 0.100041869119 -0.000655703667229\n",
      "\n",
      "sqP = \n",
      "[[1.438577716993084, 0.0, 0.0], [-0.03999096327929286, 0.28509309162103624, 0.0], [0.033769645915016175, 0.03121419662332742, 0.2551660331327631]]\n",
      "sigma = \n",
      "[[ 0.20008366  5.03762407  3.76390686]\n",
      " [ 3.0772391   5.03762407  3.76390686]\n",
      " [ 0.12010174  5.60781026  3.76390686]\n",
      " [ 0.26762295  5.10005247  4.27423893]\n",
      " [-2.67707177  5.03762407  3.76390686]\n",
      " [ 0.28006559  4.46743789  3.76390686]\n",
      " [ 0.13254437  4.97519568  3.25357479]]\n",
      "sigmax = \n",
      "[[ 0.20037818  5.13555106  3.78389271]\n",
      " [ 3.07753362  4.93787743  3.77021051]\n",
      " [ 0.12039626  5.70702099  3.77600478]\n",
      " [ 0.26791748  5.19640738  4.3007881 ]\n",
      " [-2.67677725  4.94832468  3.71902093]\n",
      " [ 0.28036011  4.56345502  3.79165286]\n",
      " [ 0.13283889  5.07424821  3.26690619]]\n",
      "sqP = \n",
      "[[1.5073969331129293, 0.0, 0.0], [-0.009393048732649801, 0.5380676281295149, 0.0], [0.018107341959517666, 0.01547488963798631, 0.516437250899869]]\n",
      "m = \n",
      "[0.2032122  5.08201139 3.85109   ]\n",
      "P = \n",
      "[[ 2.27191966 -0.01046653  0.01994941]\n",
      " [-0.01046653  0.08107252  0.0096224 ]\n",
      " [ 0.01994941  0.0096224   0.0615723 ]]\n",
      "num = \n",
      "1466\n",
      "SENSOR 1 4.45232713374 2.39852116851\n",
      "\n",
      "num = \n",
      "1467\n",
      "SENSOR 3 4.28823399159 0.800783040336\n",
      "\n",
      "num = \n",
      "1468\n",
      "SENSOR 4 4.51068601353 -2.05307475783\n",
      "\n",
      "num = \n",
      "1469\n",
      "SENSOR 7 0.901779605434 0.0337725706528\n",
      "\n",
      "num = \n",
      "1470\n",
      "SENSOR 8 0.977088757516 3.04355799617\n",
      "\n",
      "num = \n",
      "1471\n",
      "SENSOR 9 5.04440283106 -0.0103650380772\n",
      "\n",
      "num = \n",
      "1472\n",
      "ODOMETRY 0.000537988723435 0.0998524709068 -0.000469990594849\n",
      "\n",
      "sqP = \n",
      "[[1.5072888447885868, 0.0, 0.0], [-0.006943947387535825, 0.28464767185702655, 0.0], [0.013235294056297307, 0.034127479992219927, 0.24542299600381534]]\n",
      "sigma = \n",
      "[[ 0.2032122   5.08201139  3.85109   ]\n",
      " [ 3.21778989  5.08201139  3.85109   ]\n",
      " [ 0.18932431  5.65130674  3.85109   ]\n",
      " [ 0.22968279  5.15026635  4.341936  ]\n",
      " [-2.81136549  5.08201139  3.85109   ]\n",
      " [ 0.2171001   4.51271605  3.85109   ]\n",
      " [ 0.17674161  5.01375643  3.36024401]]\n",
      "sigmax = \n",
      "[[ 0.2026858   5.17999213  3.87129277]\n",
      " [-3.06592182  4.98226079  3.84346157]\n",
      " [ 0.1887979   5.74955859  3.86993012]\n",
      " [ 0.22915639  5.24767804  4.36472499]\n",
      " [-2.81189189  4.98737911  3.8186384 ]\n",
      " [ 0.21657369  4.61040677  3.87265152]\n",
      " [ 0.17621521  5.11223756  3.3778464 ]]\n",
      "sqP = \n",
      "[[1.4334599460358541, 0.0, 0.0], [0.07918544531650515, 0.5320932294829575, 0.0], [0.01854199255145697, 0.015329006658210506, 0.5105236362410347]]\n",
      "m = \n",
      "[-0.58443318  5.08600564  4.0183104 ]\n",
      "P = \n",
      "[[2.05009321 0.08330724 0.01968474]\n",
      " [0.08330724 0.08651213 0.00933052]\n",
      " [0.01968474 0.00933052 0.0461286 ]]\n",
      "num = \n",
      "1473\n",
      "SENSOR 1 4.33243080487 2.33948279246\n",
      "\n",
      "num = \n",
      "1474\n",
      "SENSOR 3 4.10533364383 0.863859372451\n",
      "\n",
      "num = \n",
      "1475\n",
      "SENSOR 4 4.60752865229 -2.16099524972\n",
      "\n",
      "num = \n",
      "1476\n",
      "SENSOR 5 5.14915760553 -1.41243997907\n",
      "\n",
      "num = \n",
      "1477\n",
      "SENSOR 7 1.00236861922 0.0941916748689\n",
      "\n",
      "num = \n",
      "1478\n",
      "SENSOR 8 1.23019120912 3.1121174408\n",
      "\n",
      "num = \n",
      "1479\n",
      "SENSOR 9 4.82508017193 0.051751886788\n",
      "\n",
      "num = \n",
      "1480\n",
      "ODOMETRY -0.000205828817179 0.100018336841 0.000249542798678\n",
      "\n",
      "sqP = \n",
      "[[1.431814655521588, 0.0, 0.0], [0.05818297623765203, 0.28831731796362475, 0.0], [0.013748104516705835, 0.029587587040677512, 0.21228322295567167]]\n",
      "sigma = \n",
      "[[-0.58443318  5.08600564  4.0183104 ]\n",
      " [ 2.27919614  5.08600564  4.0183104 ]\n",
      " [-0.46806722  5.66264028  4.0183104 ]\n",
      " [-0.55693697  5.14518082  4.44287685]\n",
      " [-3.44806249  5.08600564  4.0183104 ]\n",
      " [-0.70079913  4.50937101  4.0183104 ]\n",
      " [-0.61192938  5.02683047  3.59374396]]\n",
      "sigmax = \n",
      "[[-0.58436518  5.16931472  3.96326394]\n",
      " [ 2.27926413  5.0209989   4.0941038 ]\n",
      " [-0.46799922  5.75177703  3.97330869]\n",
      " [-0.55686897  5.22997178  4.39014158]\n",
      " [ 2.83519082  4.99078965  4.04838416]\n",
      " [-0.70073113  4.58572559  3.95396373]\n",
      " [-0.61186139  5.10859468  3.5364279 ]]\n",
      "sqP = \n",
      "[[1.4399447903499114, 0.0, 0.0], [-0.0554307022196591, 0.5365448014446207, 0.0], [0.045248532962165515, 0.013381875992417377, 0.49564113000286925]]\n",
      "m = \n",
      "[0.22953631 4.97621216 4.12630137]\n",
      "P = \n",
      "[[ 2.06926294e+00 -5.64506766e-02  4.64052564e-02]\n",
      " [-5.64506766e-02  6.13296737e-02  8.87781664e-04]\n",
      " [ 4.64052564e-02  8.87781664e-04  4.44362669e-02]]\n",
      "num = \n",
      "1481\n",
      "SENSOR 1 4.33283896214 2.46058071055\n",
      "\n",
      "num = \n",
      "1482\n",
      "SENSOR 3 4.18312790586 0.872387916195\n",
      "\n",
      "num = \n",
      "1483\n",
      "SENSOR 4 4.50465419667 -2.16382123772\n",
      "\n",
      "num = \n",
      "1484\n",
      "SENSOR 5 5.10336341986 -1.39939623537\n",
      "\n",
      "num = \n",
      "1485\n",
      "SENSOR 7 0.704062055735 0.108392416192\n",
      "\n",
      "num = \n",
      "1486\n",
      "SENSOR 8 1.20229516344 3.16630582891\n",
      "\n",
      "num = \n",
      "1487\n",
      "SENSOR 9 4.60881937906 0.0659418142304\n",
      "\n",
      "num = \n",
      "1488\n",
      "ODOMETRY -4.84769033309e-05 0.0998816841886 -0.000516660455681\n",
      "\n",
      "sqP = \n",
      "[[1.4384932892701179, 0.0, 0.0], [-0.03924291964742284, 0.24451925674697508, 0.0], [0.032259626627473656, 0.008808073557076036, 0.20812977016789533]]\n",
      "sigma = \n",
      "[[ 0.22953631  4.97621216  4.12630137]\n",
      " [ 3.10652289  4.97621216  4.12630137]\n",
      " [ 0.15105047  5.46525067  4.12630137]\n",
      " [ 0.29405556  4.99382831  4.54256091]\n",
      " [-2.64745027  4.97621216  4.12630137]\n",
      " [ 0.30802215  4.48717365  4.12630137]\n",
      " [ 0.16501705  4.95859601  3.71004183]]\n",
      "sigmax = \n",
      "[[ 0.22958002  5.0736119   4.14903809]\n",
      " [ 3.1065666   4.87625605  4.12982884]\n",
      " [ 0.15109418  5.56413325  4.14133144]\n",
      " [ 0.29409927  5.08955945  4.57153013]\n",
      " [-2.64740656  4.88814868  4.07888311]\n",
      " [ 0.30806586  4.58249087  4.15660475]\n",
      " [ 0.16506077  5.05725904  3.72645145]]\n",
      "sqP = \n",
      "[[1.5072636244293038, 0.0, 0.0], [-0.009054153100427101, 0.5167939399627599, 0.0], [0.01677649085712133, 0.004806176379231584, 0.49485432972468946]]\n",
      "m = \n",
      "[0.23288232 5.02001645 4.2318708 ]\n",
      "P = \n",
      "[[ 2.27154327 -0.00996246  0.01836556]\n",
      " [-0.00996246  0.05822194  0.0030365 ]\n",
      " [ 0.01836556  0.0030365   0.0424182 ]]\n",
      "num = \n",
      "1489\n",
      "SENSOR 1 4.50934992142 2.36066884164\n",
      "\n",
      "num = \n",
      "1490\n",
      "SENSOR 3 4.00995160209 0.872702584178\n",
      "\n",
      "num = \n",
      "1491\n",
      "SENSOR 4 4.6556628157 -2.10991460657\n",
      "\n",
      "num = \n",
      "1492\n",
      "SENSOR 5 5.19236777304 -1.46894121337\n",
      "\n",
      "num = \n",
      "1493\n",
      "SENSOR 7 0.595140527124 -0.00247210771207\n",
      "\n",
      "num = \n",
      "1494\n",
      "SENSOR 8 1.1409026588 3.07525098835\n",
      "\n",
      "num = \n",
      "1495\n",
      "SENSOR 9 4.84639214222 -0.0548253969277\n",
      "\n",
      "num = \n",
      "1496\n",
      "ODOMETRY 5.67295197279e-05 0.100061981172 0.000311855228904\n",
      "\n",
      "sqP = \n",
      "[[1.5071639815558597, 0.0, 0.0], [-0.006610070788871047, 0.24120167597726536, 0.0], [0.012185509002515726, 0.012923007454243058, 0.20518945605520442]]\n",
      "sigma = \n",
      "[[ 0.23288232  5.02001645  4.2318708 ]\n",
      " [ 3.24721028  5.02001645  4.2318708 ]\n",
      " [ 0.21966218  5.5024198   4.2318708 ]\n",
      " [ 0.25725334  5.04586246  4.64224971]\n",
      " [-2.78144565  5.02001645  4.2318708 ]\n",
      " [ 0.24610246  4.5376131   4.2318708 ]\n",
      " [ 0.2085113   4.99417043  3.82149188]]\n",
      "sigmax = \n",
      "[[ 0.23231718  5.11720297  4.25491708]\n",
      " [-3.03654016  4.92069083  4.22134595]\n",
      " [ 0.21909704  5.59990249  4.25363028]\n",
      " [ 0.2566882   5.14245852  4.66765745]\n",
      " [-2.78201078  4.92654096  4.19667584]\n",
      " [ 0.24553732  4.63448646  4.25619985]\n",
      " [ 0.20794616  5.0918897   3.84216302]]\n",
      "sqP = \n",
      "[[1.4334545965011793, 0.0, 0.0], [0.07860493983109143, 0.509176612898945, 0.0], [0.02034650762402489, 0.00493283920053876, 0.49252947683243276]]\n",
      "m = \n",
      "[-0.56846995  4.96387068  4.26378779]\n",
      "P = \n",
      "[[2.04969137 0.08162116 0.02145851]\n",
      " [0.08162116 0.06394691 0.00537581]\n",
      " [0.02145851 0.00537581 0.04428815]]\n",
      "num = \n",
      "1497\n",
      "SENSOR 1 4.62274843795 2.4285094599\n",
      "\n",
      "num = \n",
      "1498\n",
      "SENSOR 3 3.816414436 0.903117907018\n",
      "\n",
      "num = \n",
      "1499\n",
      "SENSOR 4 4.36708385287 -2.09249135014\n",
      "\n",
      "num = \n",
      "1500\n",
      "SENSOR 5 5.02112185456 -1.48480085145\n",
      "\n",
      "num = \n",
      "1501\n",
      "SENSOR 7 0.722330360765 0.131951761725\n",
      "\n",
      "num = \n",
      "1502\n",
      "SENSOR 8 1.42401805985 3.15417376672\n",
      "\n",
      "num = \n",
      "1503\n",
      "SENSOR 9 4.47099457601 -0.0163240457546\n",
      "\n",
      "num = \n",
      "1504\n",
      "ODOMETRY 0.00115916483704 0.0999765525792 -5.49301680053e-05\n",
      "\n",
      "sqP = \n",
      "[[1.43167432503313, 0.0, 0.0], [0.05701098338292937, 0.2463669178557743, 0.0], [0.014988403117698362, 0.018351931013025728, 0.20910930937514413]]\n",
      "sigma = \n",
      "[[-0.56846995  4.96387068  4.26378779]\n",
      " [ 2.2948787   4.96387068  4.26378779]\n",
      " [-0.45444798  5.45660451  4.26378779]\n",
      " [-0.53849314  5.00057454  4.68200641]\n",
      " [-3.4318186   4.96387068  4.26378779]\n",
      " [-0.68249192  4.47113684  4.26378779]\n",
      " [-0.59844676  4.92716681  3.84556917]]\n",
      "sigmax = \n",
      "[[-0.56810136  5.04819853  4.20992488]\n",
      " [ 2.29524729  4.89758053  4.33874122]\n",
      " [-0.4540794   5.54651304  4.21986904]\n",
      " [-0.53812456  5.0864789   4.6306952 ]\n",
      " [ 2.85173529  4.86799174  4.29241696]\n",
      " [-0.68212333  4.54878886  4.20068023]\n",
      " [-0.59807817  5.00984239  3.78920296]]\n",
      "sqP = \n",
      "[[1.4399238386222941, 0.0, 0.0], [-0.058227125763652035, 0.5141647829191269, 0.0], [0.044431176399780614, 0.009006361928828454, 0.4944156400756219]]\n",
      "m = \n",
      "[0.22658774 5.06794679 4.4129487 ]\n",
      "P = \n",
      "[[ 2.06905627e+00 -6.05712803e-02  4.56045673e-02]\n",
      " [-6.05712803e-02  6.16040876e-02  1.23301166e-03]\n",
      " [ 4.56045673e-02  1.23301166e-03  4.56432031e-02]]\n",
      "num = \n",
      "1505\n",
      "SENSOR 1 4.63927554993 2.3761844012\n",
      "\n",
      "num = \n",
      "1506\n",
      "SENSOR 3 3.9376280419 0.858518143986\n",
      "\n",
      "num = \n",
      "1507\n",
      "SENSOR 4 4.78153044206 -2.1711021116\n",
      "\n",
      "num = \n",
      "1508\n",
      "SENSOR 5 5.01779109603 -1.4755448089\n",
      "\n",
      "num = \n",
      "1509\n",
      "SENSOR 7 0.400487152841 0.0954002442539\n",
      "\n",
      "num = \n",
      "1510\n",
      "SENSOR 8 1.5705356409 2.92820511279\n",
      "\n",
      "num = \n",
      "1511\n",
      "SENSOR 9 4.55151136998 0.00907004840239\n",
      "\n",
      "num = \n",
      "1512\n",
      "ODOMETRY -0.00015974321687 0.100001954963 0.000375007480502\n",
      "\n",
      "sqP = \n",
      "[[1.4384214500172667, 0.0, 0.0], [-0.04210955024613798, 0.2446035023648884, 0.0], [0.031704593446023645, 0.010498941373255776, 0.21101609917057967]]\n",
      "sigma = \n",
      "[[ 0.22658774  5.06794679  4.4129487 ]\n",
      " [ 3.10343064  5.06794679  4.4129487 ]\n",
      " [ 0.14236864  5.55715379  4.4129487 ]\n",
      " [ 0.28999693  5.08894467  4.8349809 ]\n",
      " [-2.65025516  5.06794679  4.4129487 ]\n",
      " [ 0.31080684  4.57873978  4.4129487 ]\n",
      " [ 0.16317855  5.0469489   3.9909165 ]]\n",
      "sigmax = \n",
      "[[ 0.22769197  5.1653417   4.43552172]\n",
      " [ 3.10453487  4.96803867  4.41664727]\n",
      " [ 0.14347287  5.65610234  4.4272489 ]\n",
      " [ 0.29110116  5.18471347  4.86368015]\n",
      " [-2.64915093  4.97985193  4.36567706]\n",
      " [ 0.31191107  4.67389066  4.44363454]\n",
      " [ 0.16428279  5.14557846  4.00727257]]\n",
      "sqP = \n",
      "[[1.5072606487722309, 0.0, 0.0], [-0.009472845001920168, 0.5168816437759592, 0.0], [0.01677856770210473, 0.005415305029095745, 0.49606067414658744]]\n",
      "m = \n",
      "[0.23308243 5.02140791 4.53114948]\n",
      "P = \n",
      "[[ 2.27153136 -0.01038228  0.01847368]\n",
      " [-0.01038228  0.05833911  0.00460799]\n",
      " [ 0.01847368  0.00460799  0.04711613]]\n",
      "num = \n",
      "1513\n",
      "SENSOR 1 4.70778012293 2.42157051538\n",
      "\n",
      "num = \n",
      "1514\n",
      "SENSOR 3 3.97505453196 0.932447245049\n",
      "\n",
      "num = \n",
      "1515\n",
      "SENSOR 4 4.70850325866 -2.17152767966\n",
      "\n",
      "num = \n",
      "1516\n",
      "SENSOR 5 4.95124738508 -1.40632188062\n",
      "\n",
      "num = \n",
      "1517\n",
      "SENSOR 7 0.383618192004 0.149257977921\n",
      "\n",
      "num = \n",
      "1518\n",
      "SENSOR 8 1.42134250744 3.07398092328\n",
      "\n",
      "num = \n",
      "1519\n",
      "SENSOR 9 4.4606042563 -0.0161739269736\n",
      "\n",
      "num = \n",
      "1520\n",
      "ODOMETRY -0.000584883749866 0.0998918193766 0.000164194828446\n",
      "\n",
      "sqP = \n",
      "[[1.5071600315578508, 0.0, 0.0], [-0.006888640898109144, 0.24143665948829215, 0.0], [0.012257277666045606, 0.019435440974491534, 0.21584289935417328]]\n",
      "sigma = \n",
      "[[ 0.23308243  5.02140791  4.53114948]\n",
      " [ 3.24740249  5.02140791  4.53114948]\n",
      " [ 0.21930514  5.50428123  4.53114948]\n",
      " [ 0.25759698  5.06027879  4.96283528]\n",
      " [-2.78123764  5.02140791  4.53114948]\n",
      " [ 0.24685971  4.53853459  4.53114948]\n",
      " [ 0.20856787  4.98253702  4.09946368]]\n",
      "sigmax = \n",
      "[[ 0.23329769  5.1187094   4.55423215]\n",
      " [-3.03556755  4.92196354  4.5206039 ]\n",
      " [ 0.21952041  5.60189149  4.55288946]\n",
      " [ 0.25781225  5.15698524  4.98829608]\n",
      " [-2.78102237  4.92782328  4.49590309]\n",
      " [ 0.24707497  4.63550884  4.55557047]\n",
      " [ 0.20878313  5.08037509  4.12015436]]\n",
      "sqP = \n",
      "[[1.433456695371352, 0.0, 0.0], [0.07870709042182877, 0.5094949086637454, 0.0], [0.02047762340708512, 0.007938715025827214, 0.49704822106602864]]\n",
      "m = \n",
      "[-0.5521415   5.07199768  4.5346885 ]\n",
      "P = \n",
      "[[2.04971014 0.08177405 0.02182838]\n",
      " [0.08177405 0.06448707 0.00686169]\n",
      " [0.02182838 0.00686169 0.04843374]]\n",
      "num = \n",
      "1521\n",
      "SENSOR 1 4.79178293375 2.48566706534\n",
      "\n",
      "num = \n",
      "1522\n",
      "SENSOR 3 3.63177076576 0.880780831994\n",
      "\n",
      "num = \n",
      "1523\n",
      "SENSOR 4 5.01568989921 -2.26708689866\n",
      "\n",
      "num = \n",
      "1524\n",
      "SENSOR 5 4.98135878948 -1.51500189459\n",
      "\n",
      "num = \n",
      "1525\n",
      "SENSOR 7 0.270711489516 0.323697074147\n",
      "\n",
      "num = \n",
      "1526\n",
      "SENSOR 8 1.83521291936 3.07684872323\n",
      "\n",
      "num = \n",
      "1527\n",
      "SENSOR 9 4.49376890802 0.0459102111966\n",
      "\n",
      "num = \n",
      "1528\n",
      "ODOMETRY 0.000303680491478 0.100162153889 9.76063766532e-05\n",
      "\n",
      "sqP = \n",
      "[[1.4316808781818258, 0.0, 0.0], [0.05711751120947615, 0.24743617417957686, 0.0], [0.015246682474216953, 0.024211635107633297, 0.21820879611615804]]\n",
      "sigma = \n",
      "[[-0.5521415   5.07199768  4.5346885 ]\n",
      " [ 2.31122026  5.07199768  4.5346885 ]\n",
      " [-0.43790648  5.56687003  4.5346885 ]\n",
      " [-0.52164813  5.12042095  4.97110609]\n",
      " [-3.41550325  5.07199768  4.5346885 ]\n",
      " [-0.66637652  4.57712533  4.5346885 ]\n",
      " [-0.58263486  5.02357441  4.09827091]]\n",
      "sigmax = \n",
      "[[-0.55256219  5.15701524  4.48224434]\n",
      " [ 2.31079957  5.00465371  4.60846631]\n",
      " [-0.43832716  5.65731141  4.49227703]\n",
      " [-0.52206882  5.20699794  4.92127838]\n",
      " [ 2.86726136  4.9758456   4.56176531]\n",
      " [-0.66679721  4.65561083  4.47289528]\n",
      " [-0.58305555  5.10695349  4.04325906]]\n",
      "sqP = \n",
      "[[1.4399301388124024, 0.0, 0.0], [-0.05863714415767993, 0.5149311219321165, 0.0], [0.043421320967005524, 0.011894065902930913, 0.49829190129665035]]\n",
      "m = \n",
      "[0.27035609 4.95638629 4.72437391]\n",
      "P = \n",
      "[[ 2.06911331 -0.06094034  0.04460384]\n",
      " [-0.06094034  0.06165654  0.00278266]\n",
      " [ 0.04460384  0.00278266  0.04989029]]\n",
      "num = \n",
      "1529\n",
      "SENSOR 1 4.87239258496 2.49373595981\n",
      "\n",
      "num = \n",
      "1530\n",
      "SENSOR 3 3.63961037672 0.972600323588\n",
      "\n",
      "num = \n",
      "1531\n",
      "SENSOR 4 5.06493994808 -2.19537045826\n",
      "\n",
      "num = \n",
      "1532\n",
      "SENSOR 5 4.87043159977 -1.5187934091\n",
      "\n",
      "num = \n",
      "1533\n",
      "SENSOR 7 0.158485679685 0.430892408212\n",
      "\n",
      "num = \n",
      "1534\n",
      "SENSOR 8 1.94404523721 3.02999537927\n",
      "\n",
      "num = \n",
      "1535\n",
      "SENSOR 9 4.25502389275 0.0807923140829\n",
      "\n",
      "num = \n",
      "1536\n",
      "ODOMETRY -0.000118594429945 0.100038226107 0.0016860488551\n",
      "\n",
      "sqP = \n",
      "[[1.438441279538959, 0.0, 0.0], [-0.04236554169990178, 0.24466650697370304, 0.0], [0.03100845305737455, 0.016742579551416503, 0.22056393873909777]]\n",
      "sigma = \n",
      "[[ 0.27035609  4.95638629  4.72437391]\n",
      " [ 3.14723865  4.95638629  4.72437391]\n",
      " [ 0.18562501  5.4457193   4.72437391]\n",
      " [ 0.332373    4.98987145  5.16550179]\n",
      " [-2.60652647  4.95638629  4.72437391]\n",
      " [ 0.35508717  4.46705328  4.72437391]\n",
      " [ 0.20833918  4.92290113  4.28324603]]\n",
      "sigmax = \n",
      "[[ 0.27075738  5.05290201  4.75115399]\n",
      " [-3.13554537  4.85622591  4.72377798]\n",
      " [ 0.18602629  5.54415516  4.74288981]\n",
      " [ 0.33277428  5.08454187  5.19821215]\n",
      " [-2.60612518  4.87023883  4.67327527]\n",
      " [ 0.35548846  4.56095635  4.75922601]\n",
      " [ 0.20874047  5.02089106  4.30399286]]\n",
      "sqP = \n",
      "[[1.4390361520903197, 0.0, 0.0], [0.07099466196730685, 0.5122259108127266, 0.0], [0.025298283218909468, 0.005039430747754196, 0.5000057858080752]]\n",
      "m = \n",
      "[-0.51055077  4.9903831   4.86326073]\n",
      "P = \n",
      "[[2.06648051 0.07415499 0.02700466]\n",
      " [0.07415499 0.06412603 0.00777392]\n",
      " [0.02700466 0.00777392 0.05003226]]\n",
      "num = \n",
      "1537\n",
      "SENSOR 3 3.85525315557 0.972472209298\n",
      "\n",
      "num = \n",
      "1538\n",
      "SENSOR 4 4.7395963428 -2.22231607732\n",
      "\n",
      "num = \n",
      "1539\n",
      "SENSOR 5 5.03393746044 -1.59221850796\n",
      "\n",
      "num = \n",
      "1540\n",
      "SENSOR 6 5.09689220959 -0.905474203378\n",
      "\n",
      "num = \n",
      "1541\n",
      "SENSOR 7 0.210352002822 1.03187235077\n",
      "\n",
      "num = \n",
      "1542\n",
      "SENSOR 8 2.18673487592 3.14059664668\n",
      "\n",
      "num = \n",
      "1543\n",
      "SENSOR 9 4.10712293299 0.0906600190361\n",
      "\n"
     ]
    }
   ],
   "source": [
    "# Начальные распределения\n",
    "m = []\n",
    "P = []\n",
    "m.append([0,0,0])\n",
    "P.append([[1,0,0],[0,1,0],[0,0,1]])\n",
    "Y = []\n",
    "f = open('/data files/sensor_data_ekf.dat')\n",
    "X = []\n",
    "m_ = []\n",
    "p_ = []\n",
    "R = []\n",
    "n = 3\n",
    "k = 0\n",
    "nn =0 \n",
    "alpha = 1\n",
    "beta = 0\n",
    "nums = []\n",
    "kappa = 1\n",
    "# Генерация сигма точек\n",
    "delta = alpha * alpha * ( n + kappa ) - n\n",
    "wm = []\n",
    "wm.append( delta / (delta + n))\n",
    "for i in range(1,2*n+1):\n",
    "    wm.append(1 / (2*(delta + n)))\n",
    "wc = []\n",
    "wc.append(delta / (delta + n) + (1 - alpha * alpha +  beta * beta))\n",
    "for i in range(1,2*n+1):\n",
    "    wc.append(1 / (2 * (delta +  n )))\n",
    "for line in f:\n",
    "    nn = nn+1\n",
    "    print(\"num = \")\n",
    "    print(nn)\n",
    "    print(line)\n",
    "    q = line.split()\n",
    "    if q[0] == 'ODOMETRY':\n",
    "        if(len(nums)!= 0):\n",
    "            # Новый прогноз\n",
    "            # Прогноз матожидания\n",
    "            sigma = gensigma(n,m[-1],P[-1])\n",
    "            sigmax = np.array([[0.0]* n for i in range(0,2 * n + 1)])\n",
    "            for i in range(0,len(sigmax)):\n",
    "                sigma[i][0] = nrml(sigma[i][0])\n",
    "                sigmax[i] = np.array([sigma[i][0] + deltar1 + deltar2 , \n",
    "                                      sigma[i][1] + deltat * cos(sigma[i][0] + deltar1),\n",
    "                                      sigma[i][2] + deltat * sin(sigma[i][0] + deltar1)]) \n",
    "                sigmax[i][0] = nrml(sigmax[i][0])\n",
    "            m_ = [0.0] * n\n",
    "            for i in range(0,2*n+1):\n",
    "                m_ = m_ + wm[i] * sigmax[i]\n",
    "            m_[0] = nrml(m_[0])\n",
    "            # Прогноз матрицы ковариации\n",
    "            p_ =   np.array([[0.2,0,0],[0,0.2,0],[0,0,0.2]])\n",
    "            for i in range(0,2*n+1):\n",
    "                p_ = p_ + wc[i] * np.outer(np.array( sigmax[i]  - m_),np.array(sigmax[i] - m_))\n",
    "            \n",
    "            # Коррекция с конечной матрицей\n",
    "        \n",
    "            R = np.array([[0.0] * ( k)  for i in range(k)])\n",
    "            for i in range(k):\n",
    "                R[i][i] = 0.2\n",
    "            sigma = gensigma(n,m_,p_)\n",
    "            mu =  np.array([0.0]*(k))\n",
    "            S  =  R\n",
    "            C =  np.array([[0.0] * (k)  for i in range(n)])\n",
    "            sigmax = np.array([[0.0]*k for i in range(2*n+1)])\n",
    "            for i in range(2*n+1):\n",
    "                for j in range(0,k):\n",
    "                    num = nums[j]\n",
    "                    delta_x = L[num - 1][1] - sigma[i][1]\n",
    "                    delta_y = L[num - 1][2] - sigma[i][2]\n",
    "                    qq = (delta_x * delta_x) + (delta_y * delta_y)\n",
    "                    sigmax[i][j] = (math.sqrt(qq))\n",
    "\n",
    "                mu = mu + wm[i]  * sigmax[i]\n",
    "            for i in range(2*n+1):\n",
    "                S = S + wc[i] * np.outer(np.array(sigmax[i] - mu),     np.array(sigmax[i] - mu)) \n",
    "                C = C + wc[i] * np.outer(np.array(sigma[i]  - m_ ), np.array(sigmax[i] - mu))\n",
    "            \n",
    "            K = np.dot(C, np.linalg.inv(S))\n",
    "            v = np.array(Y) - np.array(mu)\n",
    "            v = np.dot(K , v.transpose())\n",
    "            v = v.tolist();\n",
    "            new_ang= m_+ v\n",
    "            new_ang[0] = nrml(new_ang[0])\n",
    "            m.append(new_ang)\n",
    "            P.append(p_  - np.dot(np.dot(K ,S),K.transpose()))\n",
    "           # print(\"m = \")\n",
    "           # print(m[-1])\n",
    "           # print(\"P = \")\n",
    "           # print(P[-1])\n",
    "        \n",
    "        #Берем дельты из Одометрии\n",
    "        deltar1 = float(q[1])\n",
    "        deltat = float(q[2])\n",
    "        deltar2 = float(q[3])\n",
    "        nums = []\n",
    "        Y = []\n",
    "        R = []\n",
    "        k = 0\n",
    "    else:\n",
    "        k = k+1\n",
    "        num = int(q[1])\n",
    "        nums.append(num)\n",
    "        Y.append(float(q[2]))\n",
    "    #    Y.append(float(q[3]))\n",
    "       "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "id": "704f9f95",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x7f5495403280>]"
      ]
     },
     "execution_count": 22,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "X =[]\n",
    "for i in range(0,len(m)):\n",
    "    X.append(m[i])#np.random.multivariate_normal(m[i],P[i]))\n",
    "x = [a[1] for a in X]\n",
    "y = [a[2] for a in X]\n",
    "fig1 = plt.figure(1)\n",
    "axes1 = fig1.subplots(1, 1)\n",
    "axes1.plot(x, y,label='parametric curve')"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0598f449",
   "metadata": {},
   "source": [
    "print(P)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "747b6dd6",
   "metadata": {},
   "source": [
    "# Сигма точечный фильтр для расстояния и угла"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "id": "d9ee774e",
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "num = \n",
      "1\n",
      "ODOMETRY 0.101373685667 0.0996252285934 0.000862235559071\n",
      "\n",
      "num = \n",
      "2\n",
      "SENSOR 1 2.29725290252 0.353956815014\n",
      "\n",
      "num = \n",
      "3\n",
      "SENSOR 2 4.01825728983 1.51507415344\n",
      "\n",
      "num = \n",
      "4\n",
      "ODOMETRY 0.101823187296 0.0997248889496 -0.000206654546125\n",
      "\n",
      "sqP = \n",
      "[[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]\n",
      "sigma = \n",
      "[[ 0.  0.  0.]\n",
      " [ 2.  0.  0.]\n",
      " [ 0.  2.  0.]\n",
      " [ 0.  0.  2.]\n",
      " [-2.  0.  0.]\n",
      " [ 0. -2.  0.]\n",
      " [ 0.  0. -2.]]\n",
      "sqP = \n",
      "[[1.0954451150103324, 0.0, 0.0], [-0.0041844252011476535, 1.0971314448189138, 0.0], [0.04113573874831966, 0.00031110853161275923, 1.0956169748066202]]\n",
      "m = \n",
      "[0.24694747 0.29175534 0.136811  ]\n",
      "P = \n",
      "[[ 0.54213942  0.03547871 -0.0242843 ]\n",
      " [ 0.03547871  0.36358715 -0.04999541]\n",
      " [-0.0242843  -0.04999541  0.20796392]]\n",
      "num = \n",
      "5\n",
      "SENSOR 1 1.96846687533 0.276209105501\n",
      "\n",
      "num = \n",
      "6\n",
      "SENSOR 2 3.87986704568 1.40393072429\n",
      "\n",
      "num = \n",
      "7\n",
      "ODOMETRY 0.100901695741 0.0997582630735 -0.000381616354179\n",
      "\n",
      "sqP = \n",
      "[[0.7363011780365769, 0.0, 0.0], [0.04818505052074062, 0.6010535364783677, 0.0], [-0.032981472047921304, -0.08053558675850468, 0.44764959264253634]]\n",
      "sigma = \n",
      "[[ 0.24694747  0.29175534  0.136811  ]\n",
      " [ 1.71954983  0.29175534  0.136811  ]\n",
      " [ 0.34331757  1.49386241  0.136811  ]\n",
      " [ 0.18098453  0.13068416  1.03211018]\n",
      " [-1.22565489  0.29175534  0.136811  ]\n",
      " [ 0.15057737 -0.91035173  0.136811  ]\n",
      " [ 0.31291041  0.45282651 -0.75848819]]\n",
      "sqP = \n",
      "[[0.8634517944681018, 0.0, 0.0], [0.022022885461394234, 0.7528207349047555, 0.0], [0.02303794761600357, -0.04438550224917979, 0.6304798754360763]]\n",
      "m = \n",
      "[0.29178015 0.36657277 0.18921786]\n",
      "P = \n",
      "[[ 0.31070391  0.04768774 -0.00998203]\n",
      " [ 0.04768774  0.21427946 -0.03125002]\n",
      " [-0.00998203 -0.03125002  0.13347015]]\n",
      "num = \n",
      "8\n",
      "SENSOR 1 1.96852886749 0.242784804425\n",
      "\n",
      "num = \n",
      "9\n",
      "SENSOR 2 3.87971065887 1.32647676387\n",
      "\n",
      "num = \n",
      "10\n",
      "ODOMETRY 0.0997651394437 0.0997993534855 -0.000208097310853\n",
      "\n",
      "sqP = \n",
      "[[0.5574082047866947, 0.0, 0.0], [0.0855526415362588, 0.45492879587873347, 0.0], [-0.017907934563921462, -0.06532438776272428, 0.3590016414960547]]\n",
      "sigma = \n",
      "[[ 0.29178015  0.36657277  0.18921786]\n",
      " [ 1.40659656  0.36657277  0.18921786]\n",
      " [ 0.46288543  1.27643036  0.18921786]\n",
      " [ 0.25596428  0.235924    0.90722114]\n",
      " [-0.82303626  0.36657277  0.18921786]\n",
      " [ 0.12067486 -0.54328482  0.18921786]\n",
      " [ 0.32759602  0.49722155 -0.52878543]]\n",
      "sqP = \n",
      "[[0.7199610096093422, 0.0, 0.0], [0.04201283138923122, 0.6381308010758209, 0.0], [0.02407669524949325, -0.033022935905611006, 0.5726145205180774]]\n",
      "m = \n",
      "[0.3416652  0.37950391 0.18772944]\n",
      "P = \n",
      "[[ 0.26078527  0.03881016 -0.00910243]\n",
      " [ 0.03881016  0.17825277 -0.02122552]\n",
      " [-0.00910243 -0.02122552  0.12001498]]\n",
      "num = \n",
      "11\n",
      "SENSOR 1 1.87508331126 0.0936569272975\n",
      "\n",
      "num = \n",
      "12\n",
      "SENSOR 2 3.849192179 1.20772931615\n",
      "\n",
      "num = \n",
      "13\n",
      "ODOMETRY 0.0988021399415 0.100360366225 -0.00122883669589\n",
      "\n",
      "sqP = \n",
      "[[0.5106713879122646, 0.0, 0.0], [0.07599829581037898, 0.4153035329258442, 0.0], [-0.01782444504067368, -0.04784668028457854, 0.34264846082968364]]\n",
      "sigma = \n",
      "[[ 0.3416652   0.37950391  0.18772944]\n",
      " [ 1.36300798  0.37950391  0.18772944]\n",
      " [ 0.4936618   1.21011097  0.18772944]\n",
      " [ 0.30601631  0.28381055  0.87302636]\n",
      " [-0.67967757  0.37950391  0.18772944]\n",
      " [ 0.18966861 -0.45110316  0.18772944]\n",
      " [ 0.37731409  0.47519727 -0.49756748]]\n",
      "sqP = \n",
      "[[0.6832852392956609, 0.0, 0.0], [0.033473406907645144, 0.6095706042943279, 0.0], [0.020619111015147365, -0.02374017632067652, 0.5629595227911597]]\n",
      "m = \n",
      "[0.43889527 0.43227162 0.2176466 ]\n",
      "P = \n",
      "[[ 0.24707354  0.03347082 -0.00960295]\n",
      " [ 0.03347082  0.16885735 -0.01872019]\n",
      " [-0.00960295 -0.01872019  0.11694987]]\n",
      "num = \n",
      "14\n",
      "SENSOR 1 1.64421872619 -0.0593514730712\n",
      "\n",
      "num = \n",
      "15\n",
      "SENSOR 2 3.95417280741 1.25970767463\n",
      "\n",
      "num = \n",
      "16\n",
      "ODOMETRY 0.0998731020684 0.0998098334361 0.000926085589796\n",
      "\n",
      "sqP = \n",
      "[[0.49706492852337814, 0.0, 0.0], [0.06733691821438333, 0.40536784538481174, 0.0], [-0.019319303201408455, -0.042971554310524164, 0.3387182904023004]]\n",
      "sigma = \n",
      "[[ 0.43889527  0.43227162  0.2176466 ]\n",
      " [ 1.43302512  0.43227162  0.2176466 ]\n",
      " [ 0.5735691   1.24300731  0.2176466 ]\n",
      " [ 0.40025666  0.34632851  0.89508318]\n",
      " [-0.55523459  0.43227162  0.2176466 ]\n",
      " [ 0.30422143 -0.37846407  0.2176466 ]\n",
      " [ 0.47753387  0.51821473 -0.45978998]]\n",
      "sqP = \n",
      "[[0.672295351165505, 0.0, 0.0], [0.025533456768362606, 0.6028232703241826, 0.0], [0.017606427500449193, -0.021369108821308172, 0.5606052250322524]]\n",
      "m = \n",
      "[0.54326407 0.61162057 0.20649873]\n",
      "P = \n",
      "[[ 0.24332268  0.03123495 -0.00998497]\n",
      " [ 0.03123495  0.16561308 -0.01614985]\n",
      " [-0.00998497 -0.01614985  0.11637156]]\n",
      "num = \n",
      "17\n",
      "SENSOR 1 1.64079501221 -0.137553375677\n",
      "\n",
      "num = \n",
      "18\n",
      "SENSOR 2 3.59759872767 1.04090139691\n",
      "\n",
      "num = \n",
      "19\n",
      "ODOMETRY 0.100314651053 0.0996200579912 0.000575268519869\n",
      "\n",
      "sqP = \n",
      "[[0.4932774880608618, 0.0, 0.0], [0.06332126301272649, 0.4019993718221672, 0.0], [-0.020242087942069536, -0.03698537747729006, 0.33851721007343516]]\n",
      "sigma = \n",
      "[[ 0.54326407  0.61162057  0.20649873]\n",
      " [ 1.52981904  0.61162057  0.20649873]\n",
      " [ 0.66990659  1.41561931  0.20649873]\n",
      " [ 0.50277989  0.53764981  0.88353315]\n",
      " [-0.44329091  0.61162057  0.20649873]\n",
      " [ 0.41662154 -0.19237818  0.20649873]\n",
      " [ 0.58374824  0.68559132 -0.47053569]]\n",
      "sqP = \n",
      "[[0.6691352663710194, 0.0, 0.0], [0.020363005556096464, 0.6002536000725152, 0.0], [0.014842010010496471, -0.018324902881448592, 0.5605484439515745]]\n",
      "m = \n",
      "[0.64550373 0.62635582 0.39790642]\n",
      "P = \n",
      "[[ 0.24397037  0.03375104 -0.01014791]\n",
      " [ 0.03375104  0.17205601 -0.01135206]\n",
      " [-0.01014791 -0.01135206  0.11503606]]\n",
      "num = \n",
      "20\n",
      "SENSOR 1 1.63668963967 -0.164205155951\n",
      "\n",
      "num = \n",
      "21\n",
      "SENSOR 2 3.76733107867 1.02783331425\n",
      "\n",
      "num = \n",
      "22\n",
      "ODOMETRY 0.100066427373 0.0997787196014 0.00016140029208\n",
      "\n",
      "sqP = \n",
      "[[0.4939335702921301, 0.0, 0.0], [0.06833112866117755, 0.40912940314489565, 0.0], [-0.02054509975515551, -0.02431551356676965, 0.33767248895654384]]\n",
      "sigma = \n",
      "[[ 0.64550373  0.62635582  0.39790642]\n",
      " [ 1.63337087  0.62635582  0.39790642]\n",
      " [ 0.78216599  1.44461462  0.39790642]\n",
      " [ 0.60441353  0.57772479  1.0732514 ]\n",
      " [-0.34236341  0.62635582  0.39790642]\n",
      " [ 0.50884147 -0.19190299  0.39790642]\n",
      " [ 0.68659393  0.67498684 -0.27743856]]\n",
      "sqP = \n",
      "[[0.6701205981982573, 0.0, 0.0], [0.021152789200018916, 0.6039159902129979, 0.0], [0.012715775861501438, -0.010990323272248272, 0.5602284523303147]]\n",
      "m = \n",
      "[0.71799906 0.65757422 0.37703219]\n",
      "P = \n",
      "[[ 0.24461969  0.03187004 -0.0102305 ]\n",
      " [ 0.03187004  0.16182396 -0.00397055]\n",
      " [-0.0102305  -0.00397055  0.11848507]]\n",
      "num = \n",
      "23\n",
      "SENSOR 1 1.35096574442 -0.290175121698\n",
      "\n",
      "num = \n",
      "24\n",
      "SENSOR 2 3.77521988295 0.983315510779\n",
      "\n",
      "num = \n",
      "25\n",
      "ODOMETRY 0.100757002056 0.0998428685737 -0.000211648847469\n",
      "\n",
      "sqP = \n",
      "[[0.49459042555965127, 0.0, 0.0], [0.06443724256156128, 0.39707908610956194, 0.0], [-0.020684789789660922, -0.006642711215638612, 0.3435303223797396]]\n",
      "sigma = \n",
      "[[ 0.71799906  0.65757422  0.37703219]\n",
      " [ 1.70717991  0.65757422  0.37703219]\n",
      " [ 0.84687354  1.45173239  0.37703219]\n",
      " [ 0.67662948  0.6442888   1.06409284]\n",
      " [-0.27118179  0.65757422  0.37703219]\n",
      " [ 0.58912457 -0.13658395  0.37703219]\n",
      " [ 0.75936864  0.67085964 -0.31002845]]\n",
      "sqP = \n",
      "[[0.6702236252273243, 0.0, 0.0], [0.01543383201617998, 0.5957063983566998, 0.0], [0.010892961838968053, -0.0014872277817661534, 0.5638755776531484]]\n",
      "m = \n",
      "[0.82597195 0.84126691 0.4127843 ]\n",
      "P = \n",
      "[[ 0.24486518  0.0308512  -0.01082338]\n",
      " [ 0.0308512   0.16125259 -0.00234023]\n",
      " [-0.01082338 -0.00234023  0.1184601 ]]\n",
      "num = \n",
      "26\n",
      "SENSOR 8 4.9185663896 -0.395438658587\n",
      "\n",
      "num = \n",
      "27\n",
      "SENSOR 1 1.34647237204 -0.509755484258\n",
      "\n",
      "num = \n",
      "28\n",
      "SENSOR 2 3.71427679672 0.92294567505\n",
      "\n",
      "num = \n",
      "29\n",
      "ODOMETRY 0.100962422572 0.0998534959103 0.000658662316867\n",
      "\n",
      "sqP = \n",
      "[[0.49483853655399856, 0.0, 0.0], [0.06234599920327283, 0.39669328775214235, 0.0], [-0.021872540492633954, -0.0024617575838461604, 0.34347581232744284]]\n",
      "sigma = \n",
      "[[ 0.82597195  0.84126691  0.4127843 ]\n",
      " [ 1.81564903  0.84126691  0.4127843 ]\n",
      " [ 0.95066395  1.63465348  0.4127843 ]\n",
      " [ 0.78222687  0.83634339  1.09973593]\n",
      " [-0.16370512  0.84126691  0.4127843 ]\n",
      " [ 0.70127995  0.04788033  0.4127843 ]\n",
      " [ 0.86971703  0.84619042 -0.27416732]]\n",
      "sqP = \n",
      "[[0.6702466776517131, 0.0, 0.0], [0.011826944108794044, 0.5954498745151738, 0.0], [0.007678620904128482, 0.0007873512198718237, 0.5638253330985794]]\n",
      "m = \n",
      "[0.94444279 0.87991227 0.45925948]\n",
      "P = \n",
      "[[ 0.20006079  0.02419989 -0.01574206]\n",
      " [ 0.02419989  0.11155914 -0.02035294]\n",
      " [-0.01574206 -0.02035294  0.10801021]]\n",
      "num = \n",
      "30\n",
      "SENSOR 8 4.87575158242 -0.471161323013\n",
      "\n",
      "num = \n",
      "31\n",
      "SENSOR 1 1.14340287924 -0.63651274602\n",
      "\n",
      "num = \n",
      "32\n",
      "SENSOR 2 3.52742272729 0.827358592505\n",
      "\n",
      "num = \n",
      "33\n",
      "ODOMETRY 0.0992390051029 0.100115677426 -0.000216811832096\n",
      "\n",
      "sqP = \n",
      "[[0.4472815565089261, 0.0, 0.0], [0.05410436704273916, 0.32959347367569364, 0.0], [-0.03519496302126315, -0.05597421344030094, 0.3219292018906648]]\n",
      "sigma = \n",
      "[[ 0.94444279  0.87991227  0.45925948]\n",
      " [ 1.8390059   0.87991227  0.45925948]\n",
      " [ 1.05265152  1.53909922  0.45925948]\n",
      " [ 0.87405286  0.76796384  1.10311789]\n",
      " [ 0.04987968  0.87991227  0.45925948]\n",
      " [ 0.83623406  0.22072532  0.45925948]\n",
      " [ 1.01483272  0.99186069 -0.18459892]]\n",
      "sqP = \n",
      "[[0.6357882971148582, 0.0, 0.0], [0.006881589991554372, 0.5563571837372324, 0.0], [-0.003753559897932696, -0.02981900923925297, 0.5497145208980942]]\n",
      "m = \n",
      "[1.04070348 0.92587768 0.59004849]\n",
      "P = \n",
      "[[ 0.19052302  0.02147091 -0.01694074]\n",
      " [ 0.02147091  0.10338704 -0.02009231]\n",
      " [-0.01694074 -0.02009231  0.10736038]]\n",
      "num = \n",
      "34\n",
      "SENSOR 8 4.72075160291 -0.539321605004\n",
      "\n",
      "num = \n",
      "35\n",
      "SENSOR 1 1.21638996495 -0.860593555215\n",
      "\n",
      "num = \n",
      "36\n",
      "SENSOR 2 3.70903844372 0.752159761487\n",
      "\n",
      "num = \n",
      "37\n",
      "ODOMETRY 0.0489965903845 0.100051905877 -0.000257629385974\n",
      "\n",
      "sqP = \n",
      "[[0.4364894230390265, 0.0, 0.0], [0.049189987555436146, 0.3177536621667834, 0.0], [-0.03881133830142777, -0.05722415415268636, 0.3202802809306958]]\n",
      "sigma = \n",
      "[[ 1.04070348  0.92587768  0.59004849]\n",
      " [ 1.91368232  0.92587768  0.59004849]\n",
      " [ 1.13908345  1.56138501  0.59004849]\n",
      " [ 0.9630808   0.81142938  1.23060905]\n",
      " [ 0.16772463  0.92587768  0.59004849]\n",
      " [ 0.9423235   0.29037036  0.59004849]\n",
      " [ 1.11832615  1.04032599 -0.05051207]]\n",
      "sqP = \n",
      "[[0.6280517425191925, 0.0, 0.0], [0.0036341345224830584, 0.5497948219015715, 0.0], [-0.008397751431226117, -0.03074919634426643, 0.5486199056399795]]\n",
      "m = \n",
      "[1.1665144 0.9924856 0.5715379]\n",
      "P = \n",
      "[[ 0.18914437  0.02027395 -0.01707133]\n",
      " [ 0.02027395  0.09759585 -0.01535924]\n",
      " [-0.01707133 -0.01535924  0.10909733]]\n",
      "num = \n",
      "38\n",
      "SENSOR 8 4.86663909002 -0.673941110611\n",
      "\n",
      "num = \n",
      "39\n",
      "SENSOR 1 1.19962918841 -0.911110534625\n",
      "\n",
      "num = \n",
      "40\n",
      "SENSOR 2 3.4305673166 0.673488499054\n",
      "\n",
      "num = \n",
      "41\n",
      "ODOMETRY 0.0998222275061 0.0996351296598 -0.000153756252698\n",
      "\n",
      "sqP = \n",
      "[[0.4349073129529751, 0.0, 0.0], [0.04661672154007431, 0.30890569346448193, 0.0], [-0.039252809336852525, -0.043797838289162935, 0.3250204514715396]]\n",
      "sigma = \n",
      "[[ 1.1665144   0.9924856   0.5715379 ]\n",
      " [ 2.03632903  0.9924856   0.5715379 ]\n",
      " [ 1.25974784  1.61029699  0.5715379 ]\n",
      " [ 1.08800878  0.90488993  1.2215788 ]\n",
      " [ 0.29669978  0.9924856   0.5715379 ]\n",
      " [ 1.07328096  0.37467422  0.5715379 ]\n",
      " [ 1.24502002  1.08008128 -0.078503  ]]\n",
      "sqP = \n",
      "[[0.6267840717726937, 0.0, 0.0], [0.0002922322443560542, 0.5437523005752399, 0.0], [-0.010921089498941668, -0.023703666389085016, 0.5517757264363125]]\n",
      "m = \n",
      "[1.21122205 0.91183158 0.67727215]\n",
      "P = \n",
      "[[ 0.1895188   0.02066753 -0.01760247]\n",
      " [ 0.02066753  0.09885846 -0.01388692]\n",
      " [-0.01760247 -0.01388692  0.10851395]]\n",
      "num = \n",
      "42\n",
      "SENSOR 8 4.58351837652 -0.750587339257\n",
      "\n",
      "num = \n",
      "43\n",
      "SENSOR 1 1.19033672649 -1.04401316016\n",
      "\n",
      "num = \n",
      "44\n",
      "SENSOR 2 3.21903139503 0.608609177764\n",
      "\n",
      "num = \n",
      "45\n",
      "ODOMETRY 0.101077500499 0.0998498505796 -0.000160756806792\n",
      "\n",
      "sqP = \n",
      "[[0.43533757492077685, 0.0, 0.0], [0.04747472817498409, 0.31081281676778644, 0.0], [-0.040434065928015094, -0.038503300370180515, 0.32464832771900964]]\n",
      "sigma = \n",
      "[[1.21122205 0.91183158 0.67727215]\n",
      " [2.0818972  0.91183158 0.67727215]\n",
      " [1.30617151 1.53345722 0.67727215]\n",
      " [1.13035392 0.83482498 1.3265688 ]\n",
      " [0.3405469  0.91183158 0.67727215]\n",
      " [1.11627259 0.29020595 0.67727215]\n",
      " [1.29209018 0.98883819 0.02797549]]\n",
      "sqP = \n",
      "[[0.6272221039158798, 0.0, 0.0], [-0.0001447027901771199, 0.5443869449296445, 0.0], [-0.013978397060084953, -0.02045387445988042, 0.5517488087319158]]\n",
      "m = \n",
      "[1.28654867 0.94366285 0.89069351]\n",
      "P = \n",
      "[[ 0.18843509  0.01798585 -0.01768592]\n",
      " [ 0.01798585  0.09304289 -0.01211448]\n",
      " [-0.01768592 -0.01211448  0.11037125]]\n",
      "num = \n",
      "46\n",
      "SENSOR 8 4.47638974681 -0.87477261163\n",
      "\n",
      "num = \n",
      "47\n",
      "SENSOR 1 0.930451391422 -1.25663412504\n",
      "\n",
      "num = \n",
      "48\n",
      "SENSOR 2 3.30661799717 0.524107985292\n",
      "\n",
      "num = \n",
      "49\n",
      "ODOMETRY 0.100521808815 0.100140214715 -0.00102153918024\n",
      "\n",
      "sqP = \n",
      "[[0.4340911061693315, 0.0, 0.0], [0.04143335233174791, 0.3022021927266605, 0.0], [-0.04074240738673988, -0.03450135629542926, 0.32790388527295394]]\n",
      "sigma = \n",
      "[[1.28654867 0.94366285 0.89069351]\n",
      " [2.15473089 0.94366285 0.89069351]\n",
      " [1.36941538 1.54806723 0.89069351]\n",
      " [1.20506386 0.87466014 1.54650128]\n",
      " [0.41836646 0.94366285 0.89069351]\n",
      " [1.20368197 0.33925846 0.89069351]\n",
      " [1.36803349 1.01266556 0.23488574]]\n",
      "sqP = \n",
      "[[0.6259486839193991, 0.0, 0.0], [-0.00426014070564208, 0.5396391643709004, 0.0], [-0.0164321714877195, -0.018613490134594034, 0.553797532325059]]\n",
      "m = \n",
      "[1.43542495 1.06039909 0.95564534]\n",
      "P = \n",
      "[[ 0.19353185  0.01863615 -0.01426369]\n",
      " [ 0.01863615  0.08354255 -0.00419558]\n",
      " [-0.01426369 -0.00419558  0.11450099]]\n",
      "num = \n",
      "50\n",
      "SENSOR 8 4.42544858746 -1.03059237397\n",
      "\n",
      "num = \n",
      "51\n",
      "SENSOR 1 1.09621121037 -1.43900397462\n",
      "\n",
      "num = \n",
      "52\n",
      "SENSOR 2 3.2382990193 0.518547924017\n",
      "\n",
      "num = \n",
      "53\n",
      "ODOMETRY 0.0991500821073 0.10013162298 0.00061821939171\n",
      "\n",
      "sqP = \n",
      "[[0.43992255063228874, 0.0, 0.0], [0.04236235166428904, 0.2859160350703032, 0.0], [-0.03242319459068034, -0.009870217573005416, 0.33667834090914456]]\n",
      "sigma = \n",
      "[[1.43542495 1.06039909 0.95564534]\n",
      " [2.31527006 1.06039909 0.95564534]\n",
      " [1.52014966 1.63223116 0.95564534]\n",
      " [1.37057856 1.04065866 1.62900202]\n",
      " [0.55557985 1.06039909 0.95564534]\n",
      " [1.35070025 0.48856702 0.95564534]\n",
      " [1.50027134 1.08013953 0.28228865]]\n",
      "sqP = \n",
      "[[0.6295853261797106, 0.0, 0.0], [-0.007651038791073367, 0.5299219088749766, 0.0], [-0.016383515463020474, -0.004449155004588242, 0.5596734228909848]]\n",
      "m = \n",
      "[1.53838465 1.05173524 1.03204622]\n",
      "P = \n",
      "[[ 0.20005536  0.0206205  -0.01220163]\n",
      " [ 0.0206205   0.08279234  0.0012726 ]\n",
      " [-0.01220163  0.0012726   0.11669134]]\n",
      "num = \n",
      "54\n",
      "SENSOR 8 4.48032266707 -1.16853964317\n",
      "\n",
      "num = \n",
      "55\n",
      "SENSOR 1 1.05453646597 -1.57360343045\n",
      "\n",
      "num = \n",
      "56\n",
      "SENSOR 2 3.12147771546 0.369604279194\n",
      "\n",
      "num = \n",
      "57\n",
      "ODOMETRY 0.000277686359907 0.0999909476789 -5.65207074877e-05\n",
      "\n",
      "sqP = \n",
      "[[0.44727548821925495, 0.0, 0.0], [0.04610246586482677, 0.2840191997668237, 0.0], [-0.02727990440860982, 0.008908793215135349, 0.3403935732908108]]\n",
      "sigma = \n",
      "[[1.53838465 1.05173524 1.03204622]\n",
      " [2.43293563 1.05173524 1.03204622]\n",
      " [1.63058958 1.61977364 1.03204622]\n",
      " [1.48382484 1.06955283 1.71283336]\n",
      " [0.64383367 1.05173524 1.03204622]\n",
      " [1.44617972 0.48369684 1.03204622]\n",
      " [1.59294446 1.03391766 0.35125907]]\n",
      "sqP = \n",
      "[[0.6347637299855224, 0.0, 0.0], [-0.0076589492033291625, 0.5288394174955641, 0.0], [-0.01649396617895819, 0.007249448053811495, 0.5620795028258854]]\n",
      "m = \n",
      "[1.64940636 1.01825266 1.10581907]\n",
      "P = \n",
      "[[ 0.20451328  0.02088685 -0.0105583 ]\n",
      " [ 0.02088685  0.08060546  0.00398632]\n",
      " [-0.0105583   0.00398632  0.11835578]]\n",
      "num = \n",
      "58\n",
      "SENSOR 8 4.51041519435 -1.13505398802\n",
      "\n",
      "num = \n",
      "59\n",
      "SENSOR 1 1.01408592378 -1.6209817432\n",
      "\n",
      "num = \n",
      "60\n",
      "SENSOR 2 3.0349959581 0.307197125698\n",
      "\n",
      "num = \n",
      "61\n",
      "ODOMETRY -0.000206028015542 0.100097050614 0.00122219552996\n",
      "\n",
      "sqP = \n",
      "[[0.4522314452535227, 0.0, 0.0], [0.04618619847405545, 0.2801290699494148, 0.0], [-0.023347110111840996, 0.018079632462885114, 0.342759132168191]]\n",
      "sigma = \n",
      "[[1.64940636 1.01825266 1.10581907]\n",
      " [2.55386925 1.01825266 1.10581907]\n",
      " [1.74177875 1.5785108  1.10581907]\n",
      " [1.60271214 1.05441192 1.79133734]\n",
      " [0.74494347 1.01825266 1.10581907]\n",
      " [1.55703396 0.45799452 1.10581907]\n",
      " [1.69610058 0.98209339 0.42030081]]\n",
      "sqP = \n",
      "[[0.6381156106507263, 0.0, 0.0], [-0.00856988491076619, 0.5270648811550706, 0.0], [-0.014768714166556134, 0.013037230329176669, 0.5634720614136554]]\n",
      "m = \n",
      "[1.660868   0.99025278 1.16547654]\n",
      "P = \n",
      "[[ 0.20747703  0.02066668 -0.00882427]\n",
      " [ 0.02066668  0.07877864  0.0058512 ]\n",
      " [-0.00882427  0.0058512   0.11945231]]\n",
      "num = \n",
      "62\n",
      "SENSOR 8 4.47730873513 -1.08933708692\n",
      "\n",
      "num = \n",
      "63\n",
      "SENSOR 1 0.977458402036 -1.79104939149\n",
      "\n",
      "num = \n",
      "64\n",
      "SENSOR 2 2.7939870742 0.344183271835\n",
      "\n",
      "num = \n",
      "65\n",
      "ODOMETRY -0.000589698440506 0.100045946005 -1.26097962524e-05\n",
      "\n",
      "sqP = \n",
      "[[0.45549646537134997, 0.0, 0.0], [0.045371758974474444, 0.2769838248959293, 0.0], [-0.019372851644396276, 0.02429808110084952, 0.3442188384606116]]\n",
      "sigma = \n",
      "[[1.660868   0.99025278 1.16547654]\n",
      " [2.57186093 0.99025278 1.16547654]\n",
      " [1.75161152 1.54422043 1.16547654]\n",
      " [1.6221223  1.03884894 1.85391422]\n",
      " [0.74987507 0.99025278 1.16547654]\n",
      " [1.57012448 0.43628513 1.16547654]\n",
      " [1.6996137  0.94165662 0.47703886]]\n",
      "sqP = \n",
      "[[0.6402428709943337, 0.0, 0.0], [-0.009504662101427466, 0.525727251298356, 0.0], [-0.012974503663255574, 0.016954920328973864, 0.5643050699314553]]\n",
      "m = \n",
      "[1.48205845 0.95068998 1.33427096]\n",
      "P = \n",
      "[[ 0.19413972 -0.00785161  0.0016208 ]\n",
      " [-0.00785161  0.08269571  0.00711812]\n",
      " [ 0.0016208   0.00711812  0.11968876]]\n",
      "num = \n",
      "66\n",
      "SENSOR 8 4.31671160573 -1.13337841163\n",
      "\n",
      "num = \n",
      "67\n",
      "SENSOR 1 0.886564013958 -1.96817341764\n",
      "\n",
      "num = \n",
      "68\n",
      "SENSOR 2 2.8062997236 0.378866179691\n",
      "\n",
      "num = \n",
      "69\n",
      "ODOMETRY 0.000268189622775 0.099944716103 0.000225158589822\n",
      "\n",
      "sqP = \n",
      "[[0.4406128928107152, 0.0, 0.0], [-0.01781974306417179, 0.2870159709203623, 0.0], [0.0036785196784013346, 0.025028825139160292, 0.3450344684313555]]\n",
      "sigma = \n",
      "[[1.48205845 0.95068998 1.33427096]\n",
      " [2.36328423 0.95068998 1.33427096]\n",
      " [1.44641896 1.52472193 1.33427096]\n",
      " [1.48941548 1.00074763 2.0243399 ]\n",
      " [0.60083266 0.95068998 1.33427096]\n",
      " [1.51769793 0.37665804 1.33427096]\n",
      " [1.47470141 0.90063233 0.64420203]]\n",
      "sqP = \n",
      "[[0.6280691013423001, 0.0, 0.0], [-0.035016120892940004, 0.5331623543132303, 0.0], [0.004440841804965911, 0.015960286113931126, 0.5648495109300435]]\n",
      "m = \n",
      "[1.49578074 1.09088451 1.38206318]\n",
      "P = \n",
      "[[ 0.18433639 -0.00857039 -0.01578613]\n",
      " [-0.00857039  0.08977434  0.00506887]\n",
      " [-0.01578613  0.00506887  0.114178  ]]\n",
      "num = \n",
      "70\n",
      "SENSOR 8 4.44217614349 -1.17703179109\n",
      "\n",
      "num = \n",
      "71\n",
      "SENSOR 1 1.00399386332 -1.97871776411\n",
      "\n",
      "num = \n",
      "72\n",
      "SENSOR 2 2.87352149613 0.402848100533\n",
      "\n",
      "num = \n",
      "73\n",
      "ODOMETRY -0.000740364779592 0.0999166193458 0.000304584312929\n",
      "\n",
      "sqP = \n",
      "[[0.42934414270204224, 0.0, 0.0], [-0.019961574509906592, 0.2989579836632118, 0.0], [-0.03676801295965562, 0.014500104164807882, 0.33558286865512266]]\n",
      "sigma = \n",
      "[[1.49578074 1.09088451 1.38206318]\n",
      " [2.35446903 1.09088451 1.38206318]\n",
      " [1.45585759 1.68880048 1.38206318]\n",
      " [1.42224472 1.11988472 2.05322892]\n",
      " [0.63709246 1.09088451 1.38206318]\n",
      " [1.53570389 0.49296854 1.38206318]\n",
      " [1.56931677 1.0618843  0.71089744]]\n",
      "sqP = \n",
      "[[0.6213587885485065, 0.0, 0.0], [-0.03680831972365628, 0.5394147687233753, 0.0], [-0.017884658034144425, 0.00981914508592724, 0.5587896331950097]]\n",
      "m = \n",
      "[1.38677219 0.97889145 1.36884237]\n",
      "P = \n",
      "[[ 0.18567307 -0.01457988 -0.00038182]\n",
      " [-0.01457988  0.08922704  0.01523822]\n",
      " [-0.00038182  0.01523822  0.12035467]]\n",
      "num = \n",
      "74\n",
      "SENSOR 8 4.41002211254 -1.2055058086\n",
      "\n",
      "num = \n",
      "75\n",
      "SENSOR 1 1.20240484656 -2.09055794649\n",
      "\n",
      "num = \n",
      "76\n",
      "SENSOR 2 2.61257298591 0.400689815362\n",
      "\n",
      "num = \n",
      "77\n",
      "ODOMETRY -0.00093907783548 0.0999937264589 -0.000488136208846\n",
      "\n",
      "sqP = \n",
      "[[0.43089798492518794, 0.0, 0.0], [-0.033836026141804976, 0.29678638859591355, 0.0], [-0.0008861128676326264, 0.05124303871832645, 0.3431151863243551]]\n",
      "sigma = \n",
      "[[1.38677219 0.97889145 1.36884237]\n",
      " [2.24856816 0.97889145 1.36884237]\n",
      " [1.31910014 1.57246422 1.36884237]\n",
      " [1.38499997 1.08137752 2.05507274]\n",
      " [0.52497622 0.97889145 1.36884237]\n",
      " [1.45444425 0.38531867 1.36884237]\n",
      " [1.38854442 0.87640537 0.682612  ]]\n",
      "sqP = \n",
      "[[0.6219475341808749, 0.0, 0.0], [-0.04222388338141854, 0.5406599363653648, 0.0], [0.004371328072974512, 0.0321661151255426, 0.562966489266111]]\n",
      "m = \n",
      "[1.47694885 0.9580318  1.52614291]\n",
      "P = \n",
      "[[ 0.18234646 -0.01021093 -0.01537554]\n",
      " [-0.01021093  0.09150753  0.00714697]\n",
      " [-0.01537554  0.00714697  0.11518114]]\n",
      "num = \n",
      "78\n",
      "SENSOR 8 4.28220992404 -1.22818070834\n",
      "\n",
      "num = \n",
      "79\n",
      "SENSOR 1 1.35806022311 -2.03308175218\n",
      "\n",
      "num = \n",
      "80\n",
      "SENSOR 2 2.41796456399 0.428960172356\n",
      "\n",
      "num = \n",
      "81\n",
      "ODOMETRY 0.000672986065937 0.0999170760041 9.59803128141e-05\n",
      "\n",
      "sqP = \n",
      "[[0.42702044429469377, 0.0, 0.0], [-0.023912029190320334, 0.30155553395208623, 0.0], [-0.036006562410906454, 0.020845172249742888, 0.3368236071983574]]\n",
      "sigma = \n",
      "[[1.47694885 0.9580318  1.52614291]\n",
      " [2.33098973 0.9580318  1.52614291]\n",
      " [1.42912479 1.56114287 1.52614291]\n",
      " [1.40493572 0.99972215 2.19979013]\n",
      " [0.62290796 0.9580318  1.52614291]\n",
      " [1.5247729  0.35492074 1.52614291]\n",
      " [1.54896197 0.91634146 0.8524957 ]]\n",
      "sqP = \n",
      "[[0.619850560637229, 0.0, 0.0], [-0.038995463877467086, 0.5411677891984117, 0.0], [-0.01707952430078087, 0.013620613167567498, 0.5594403791220309]]\n",
      "m = \n",
      "[1.42793497 0.96752274 1.75207838]\n",
      "P = \n",
      "[[0.18469117 0.00934063 0.00561285]\n",
      " [0.00934063 0.09496411 0.01434736]\n",
      " [0.00561285 0.01434736 0.11709575]]\n",
      "num = \n",
      "82\n",
      "SENSOR 8 4.22916152226 -1.1920443901\n",
      "\n",
      "num = \n",
      "83\n",
      "SENSOR 1 1.43456173466 -2.20151685421\n",
      "\n",
      "num = \n",
      "84\n",
      "SENSOR 2 2.54268409924 0.418576167122\n",
      "\n",
      "num = \n",
      "85\n",
      "ODOMETRY -0.000190086120117 0.0998500295775 -0.00120829073569\n",
      "\n",
      "sqP = \n",
      "[[0.42975710191795635, 0.0, 0.0], [0.021734667127640127, 0.3073950476778225, 0.0], [0.013060527999466196, 0.04575056068043985, 0.33886879210777615]]\n",
      "sigma = \n",
      "[[1.42793497 0.96752274 1.75207838]\n",
      " [2.28744917 0.96752274 1.75207838]\n",
      " [1.4714043  1.58231284 1.75207838]\n",
      " [1.45405603 1.05902386 2.42981596]\n",
      " [0.56842077 0.96752274 1.75207838]\n",
      " [1.38446564 0.35273264 1.75207838]\n",
      " [1.40181391 0.87602162 1.07434079]]\n",
      "sqP = \n",
      "[[0.6207528814235943, 0.0, 0.0], [-0.014313088519429236, 0.5443848164923624, 0.0], [0.010857280915887434, 0.027836795853502138, 0.5606371983289579]]\n",
      "m = \n",
      "[1.38457905 1.00828896 1.80548281]\n",
      "P = \n",
      "[[0.18751906 0.01493767 0.01227116]\n",
      " [0.01493767 0.09666325 0.02070873]\n",
      " [0.01227116 0.02070873 0.11745898]]\n",
      "num = \n",
      "86\n",
      "SENSOR 8 4.25479049036 -1.29393177827\n",
      "\n",
      "num = \n",
      "87\n",
      "SENSOR 1 1.3915222382 -2.32656801851\n",
      "\n",
      "num = \n",
      "88\n",
      "SENSOR 2 2.22979301015 0.456482047365\n",
      "\n",
      "num = \n",
      "89\n",
      "ODOMETRY 0.000473163132143 0.09995123511 -0.000781756651883\n",
      "\n",
      "sqP = \n",
      "[[0.4330347099942679, 0.0, 0.0], [0.03449531560445818, 0.3089875813521179, 0.0], [0.028337579849117042, 0.06385763175533947, 0.3355266929007977]]\n",
      "sigma = \n",
      "[[1.38457905 1.00828896 1.80548281]\n",
      " [2.25064847 1.00828896 1.80548281]\n",
      " [1.45356968 1.62626413 1.80548281]\n",
      " [1.44125421 1.13600423 2.4765362 ]\n",
      " [0.51850963 1.00828896 1.80548281]\n",
      " [1.31558842 0.3903138  1.80548281]\n",
      " [1.32790389 0.8805737  1.13442943]]\n",
      "sqP = \n",
      "[[0.6241089690832651, 0.0, 0.0], [-0.006266882865860845, 0.5463430895431496, 0.0], [0.02018416839163277, 0.03775177500207323, 0.5580052376400872]]\n",
      "m = \n",
      "[1.34940227 0.99990413 1.96638343]\n",
      "P = \n",
      "[[0.18941444 0.01678755 0.01468797]\n",
      " [0.01678755 0.09753576 0.02303799]\n",
      " [0.01468797 0.02303799 0.11833828]]\n",
      "num = \n",
      "90\n",
      "SENSOR 8 4.18161649136 -1.28816567381\n",
      "\n",
      "num = \n",
      "91\n",
      "SENSOR 1 1.3025278855 -2.3472367671\n",
      "\n",
      "num = \n",
      "92\n",
      "SENSOR 2 2.30576040901 0.369157306239\n",
      "\n",
      "num = \n",
      "93\n",
      "ODOMETRY -0.000559278187115 0.100028723409 9.13999205286e-06\n",
      "\n",
      "sqP = \n",
      "[[0.4352176869600453, 0.0, 0.0], [0.03857276880954576, 0.30991596636964985, 0.0], [0.033748554382617126, 0.0701358420064055, 0.3350824967283124]]\n",
      "sigma = \n",
      "[[1.34940227 0.99990413 1.96638343]\n",
      " [2.21983764 0.99990413 1.96638343]\n",
      " [1.42654781 1.61973607 1.96638343]\n",
      " [1.41689938 1.14017582 2.63654843]\n",
      " [0.47896689 0.99990413 1.96638343]\n",
      " [1.27225673 0.3800722  1.96638343]\n",
      " [1.28190516 0.85963245 1.29621844]]\n",
      "sqP = \n",
      "[[0.6261319816615368, 0.0, 0.0], [-0.0034512314957849295, 0.547348734030972, 0.0], [0.023972959340548293, 0.04115792757161162, 0.5574954069407825]]\n",
      "m = \n",
      "[1.33796417 1.06217659 1.97592215]\n",
      "P = \n",
      "[[0.19016414 0.01736304 0.0159654 ]\n",
      " [0.01736304 0.09896931 0.02707253]\n",
      " [0.0159654  0.02707253 0.11872568]]\n",
      "num = \n",
      "94\n",
      "SENSOR 8 4.17489666144 -1.34658482855\n",
      "\n",
      "num = \n",
      "95\n",
      "SENSOR 1 1.3978512621 -2.36482377797\n",
      "\n",
      "num = \n",
      "96\n",
      "SENSOR 2 2.11259117923 0.471244681669\n",
      "\n",
      "num = \n",
      "97\n",
      "SENSOR 7 5.05551177517 -0.919442363958\n",
      "\n",
      "num = \n",
      "98\n",
      "ODOMETRY 0.000257701615542 0.10004347626 -0.000159471500187\n",
      "\n",
      "sqP = \n",
      "[[0.43607813671431395, 0.0, 0.0], [0.03981635045205221, 0.31206404808750543, 0.0], [0.03661132124678296, 0.08208187862103629, 0.3326377301722327]]\n",
      "sigma = \n",
      "[[1.33796417 1.06217659 1.97592215]\n",
      " [2.21012044 1.06217659 1.97592215]\n",
      " [1.41759687 1.68630468 1.97592215]\n",
      " [1.41118681 1.22634034 2.64119761]\n",
      " [0.4658079  1.06217659 1.97592215]\n",
      " [1.25833147 0.43804849 1.97592215]\n",
      " [1.26474153 0.89801283 1.31064669]]\n",
      "sqP = \n",
      "[[0.6269687966135027, 0.0, 0.0], [-0.0017571966333385493, 0.5500423803453437, 0.0], [0.025692464027725143, 0.04770136642251442, 0.5554988427685319]]\n",
      "m = \n",
      "[1.35504697 1.01509851 2.06965162]\n",
      "P = \n",
      "[[0.15895391 0.01073598 0.00671448]\n",
      " [0.01073598 0.07130532 0.00123717]\n",
      " [0.00671448 0.00123717 0.09355898]]\n",
      "num = \n",
      "99\n",
      "SENSOR 8 3.962669916 -1.30912369209\n",
      "\n",
      "num = \n",
      "100\n",
      "SENSOR 1 1.70916168099 -2.46045075948\n",
      "\n",
      "num = \n",
      "101\n",
      "SENSOR 2 2.02595774502 0.554005367169\n",
      "\n",
      "num = \n",
      "102\n",
      "SENSOR 3 4.7371187823 -0.173423905805\n",
      "\n",
      "num = \n",
      "103\n",
      "SENSOR 7 4.82701773051 -0.911583275309\n",
      "\n",
      "num = \n",
      "104\n",
      "ODOMETRY -0.000132246838171 0.100204785792 0.000451918927672\n",
      "\n",
      "sqP = \n",
      "[[0.3986902425796088, 0.0, 0.0], [0.02692812219585328, 0.2656693337334021, 0.0], [0.0168413348402498, 0.002949773985024191, 0.305395889155274]]\n",
      "sigma = \n",
      "[[1.35504697 1.01509851 2.06965162]\n",
      " [2.15242746 1.01509851 2.06965162]\n",
      " [1.40890322 1.54643718 2.06965162]\n",
      " [1.38872964 1.02099806 2.6804434 ]\n",
      " [0.55766649 1.01509851 2.06965162]\n",
      " [1.30119073 0.48375984 2.06965162]\n",
      " [1.3213643  1.00919896 1.45885984]]\n",
      "sqP = \n",
      "[[0.5999688857369052, 0.0, 0.0], [-0.011391861946025823, 0.519895815737653, 0.0], [0.013694291306604834, 0.0009118754843968858, 0.5417742196535172]]\n",
      "m = \n",
      "[1.37995117 1.07493463 2.32675132]\n",
      "P = \n",
      "[[ 0.13103329  0.00952668  0.00287933]\n",
      " [ 0.00952668  0.06954857 -0.00302373]\n",
      " [ 0.00287933 -0.00302373  0.06226421]]\n",
      "num = \n",
      "105\n",
      "SENSOR 8 3.98259516004 -1.43618538277\n",
      "\n",
      "num = \n",
      "106\n",
      "SENSOR 1 1.79511416601 -2.47746949194\n",
      "\n",
      "num = \n",
      "107\n",
      "SENSOR 2 2.06053687043 0.463761690139\n",
      "\n",
      "num = \n",
      "108\n",
      "SENSOR 3 4.8224768036 -0.239591007404\n",
      "\n",
      "num = \n",
      "109\n",
      "SENSOR 7 4.77817974323 -1.00970624708\n",
      "\n",
      "num = \n",
      "110\n",
      "ODOMETRY 0.000547071844751 0.100153178582 -0.00035008972123\n",
      "\n",
      "sqP = \n",
      "[[0.36198521278515167, 0.0, 0.0], [0.02631786439584682, 0.26240415484126317, 0.0], [0.007954274180918755, -0.012320945976071866, 0.2490966297529551]]\n",
      "sigma = \n",
      "[[1.37995117 1.07493463 2.32675132]\n",
      " [2.1039216  1.07493463 2.32675132]\n",
      " [1.4325869  1.59974294 2.32675132]\n",
      " [1.39585972 1.05029274 2.82494458]\n",
      " [0.65598075 1.07493463 2.32675132]\n",
      " [1.32731544 0.55012632 2.32675132]\n",
      " [1.36404262 1.09957653 1.82855807]]\n",
      "sqP = \n",
      "[[0.5760114536528028, 0.0, 0.0], [-0.008786069821980814, 0.5183288579342614, 0.0], [0.007423495581140362, -0.00627797415355891, 0.5120387217723017]]\n",
      "m = \n",
      "[1.43409793 1.07715198 2.3741784 ]\n",
      "P = \n",
      "[[ 0.12515632  0.00933827  0.00238424]\n",
      " [ 0.00933827  0.06776678 -0.00092572]\n",
      " [ 0.00238424 -0.00092572  0.0601489 ]]\n",
      "num = \n",
      "111\n",
      "SENSOR 8 3.83264320308 -1.36158444573\n",
      "\n",
      "num = \n",
      "112\n",
      "SENSOR 1 1.80852245484 -2.51982496643\n",
      "\n",
      "num = \n",
      "113\n",
      "SENSOR 2 1.96102088098 0.610994481673\n",
      "\n",
      "num = \n",
      "114\n",
      "SENSOR 3 4.85211156128 -0.1489018205\n",
      "\n",
      "num = \n",
      "115\n",
      "SENSOR 7 4.71279477962 -0.937135174382\n",
      "\n",
      "num = \n",
      "116\n",
      "ODOMETRY -0.000479308539659 0.0998432674256 -0.000777162798183\n",
      "\n",
      "sqP = \n",
      "[[0.35377439495676033, 0.0, 0.0], [0.026396109333275356, 0.25897881769576997, 0.0], [0.00673943300973281, -0.004261391924331775, 0.24512306260001177]]\n",
      "sigma = \n",
      "[[1.43409793 1.07715198 2.3741784 ]\n",
      " [2.14164672 1.07715198 2.3741784 ]\n",
      " [1.48689014 1.59510962 2.3741784 ]\n",
      " [1.44757679 1.0686292  2.86442453]\n",
      " [0.72654914 1.07715198 2.3741784 ]\n",
      " [1.38130571 0.55919435 2.3741784 ]\n",
      " [1.42061906 1.08567476 1.88393228]]\n",
      "sqP = \n",
      "[[0.5708752027126848, 0.0, 0.0], [-0.0081883673262462, 0.5164494268900077, 0.0], [0.0056492053516217205, -0.0023209987892710148, 0.5101161831342949]]\n",
      "m = \n",
      "[1.41781079 1.12402698 2.41771018]\n",
      "P = \n",
      "[[ 0.12387118  0.00921585  0.00221337]\n",
      " [ 0.00921585  0.06725976 -0.0005061 ]\n",
      " [ 0.00221337 -0.0005061   0.06021068]]\n",
      "num = \n",
      "117\n",
      "SENSOR 8 3.98024682642 -1.42089195757\n",
      "\n",
      "num = \n",
      "118\n",
      "SENSOR 1 1.68616417487 -2.51982610203\n",
      "\n",
      "num = \n",
      "119\n",
      "SENSOR 2 1.84497621719 0.568224090854\n",
      "\n",
      "num = \n",
      "120\n",
      "SENSOR 3 4.61852185259 -0.195327764501\n",
      "\n",
      "num = \n",
      "121\n",
      "SENSOR 7 4.6199153731 -0.979883951883\n",
      "\n",
      "num = \n",
      "122\n",
      "ODOMETRY 5.14521975978e-05 0.0998424550095 -0.00128773957906\n",
      "\n",
      "sqP = \n",
      "[[0.3519533792012119, 0.0, 0.0], [0.026184867621215382, 0.2580195960941363, 0.0], [0.006288817430550075, -0.0025996943352908394, 0.24528427572599282]]\n",
      "sigma = \n",
      "[[1.41781079 1.12402698 2.41771018]\n",
      " [2.12171755 1.12402698 2.41771018]\n",
      " [1.47018053 1.64006617 2.41771018]\n",
      " [1.43038843 1.11882759 2.90827874]\n",
      " [0.71390403 1.12402698 2.41771018]\n",
      " [1.36544106 0.60798779 2.41771018]\n",
      " [1.40523316 1.12922637 1.92714163]]\n",
      "sqP = \n",
      "[[0.5697336023512808, 0.0, 0.0], [-0.008020231720774327, 0.5159613597473669, 0.0], [0.005777921177492397, -0.0015187287890126781, 0.5101990184381414]]\n",
      "m = \n",
      "[1.42611837 1.12290268 2.50238217]\n",
      "P = \n",
      "[[ 0.12353169  0.00911172  0.0022148 ]\n",
      " [ 0.00911172  0.06707911 -0.00019227]\n",
      " [ 0.0022148  -0.00019227  0.06015788]]\n",
      "num = \n",
      "123\n",
      "SENSOR 8 4.07638490671 -1.48596910761\n",
      "\n",
      "num = \n",
      "124\n",
      "SENSOR 1 2.10918785502 -2.53013604019\n",
      "\n",
      "num = \n",
      "125\n",
      "SENSOR 2 1.87374300045 0.596795933857\n",
      "\n",
      "num = \n",
      "126\n",
      "SENSOR 3 4.63925938223 -0.116829211894\n",
      "\n",
      "num = \n",
      "127\n",
      "SENSOR 7 4.74670921432 -1.04281914214\n",
      "\n",
      "num = \n",
      "128\n",
      "ODOMETRY 0.000828058608643 0.0998754656366 0.000718626272025\n",
      "\n",
      "sqP = \n",
      "[[0.3514707562269179, 0.0, 0.0], [0.02592453620328375, 0.2576956211639149, 0.0], [0.006301525182019659, -0.0013800379311583448, 0.24518617688172775]]\n",
      "sigma = \n",
      "[[1.42611837 1.12290268 2.50238217]\n",
      " [2.12905989 1.12290268 2.50238217]\n",
      " [1.47796745 1.63829393 2.50238217]\n",
      " [1.43872143 1.12014261 2.99275453]\n",
      " [0.72317686 1.12290268 2.50238217]\n",
      " [1.3742693  0.60751144 2.50238217]\n",
      " [1.41351532 1.12566276 2.01200982]]\n",
      "sqP = \n",
      "[[0.5694238169234727, 0.0, 0.0], [-0.00811821461194859, 0.5158044498222689, 0.0], [0.00560222019590321, -0.000938722664857024, 0.5101496689303582]]\n",
      "m = \n",
      "[1.4288365  1.00093494 2.59518536]\n",
      "P = \n",
      "[[0.12342731 0.00890027 0.00230951]\n",
      " [0.00890027 0.06636019 0.00021175]\n",
      " [0.00230951 0.00021175 0.06059054]]\n",
      "num = \n",
      "129\n",
      "SENSOR 8 3.98430154528 -1.48232363932\n",
      "\n",
      "num = \n",
      "130\n",
      "SENSOR 1 1.95702304621 -2.53750111924\n",
      "\n",
      "num = \n",
      "131\n",
      "SENSOR 2 1.58489922833 0.698949835685\n",
      "\n",
      "num = \n",
      "132\n",
      "SENSOR 3 4.4671595916 -0.168854291814\n",
      "\n",
      "num = \n",
      "133\n",
      "SENSOR 7 4.52930108719 -1.12389445264\n",
      "\n",
      "num = \n",
      "134\n",
      "ODOMETRY -0.000158830047539 0.100032780056 -0.000437720533696\n",
      "\n",
      "sqP = \n",
      "[[0.3513222280002269, 0.0, 0.0], [0.02533365071919145, 0.25635600326767655, 0.0], [0.006573766696867132, 0.0001763524385258959, 0.24606360186170426]]\n",
      "sigma = \n",
      "[[1.4288365  1.00093494 2.59518536]\n",
      " [2.13148095 1.00093494 2.59518536]\n",
      " [1.4795038  1.51364695 2.59518536]\n",
      " [1.44198403 1.00128765 3.08731256]\n",
      " [0.72619204 1.00093494 2.59518536]\n",
      " [1.3781692  0.48822294 2.59518536]\n",
      " [1.41568896 1.00058224 2.10305815]]\n",
      "sqP = \n",
      "[[0.56930862996655, 0.0, 0.0], [-0.008426257319945566, 0.515163074414244, 0.0], [0.005659487066845256, -0.00021173397729134304, 0.5105721067370848]]\n",
      "m = \n",
      "[1.42971432 1.04368266 2.73105881]\n",
      "P = \n",
      "[[0.12349927 0.00877472 0.00269489]\n",
      " [0.00877472 0.0652595  0.00044191]\n",
      " [0.00269489 0.00044191 0.06170347]]\n",
      "num = \n",
      "135\n",
      "SENSOR 8 3.84481274007 -1.42332717446\n",
      "\n",
      "num = \n",
      "136\n",
      "SENSOR 1 1.79980984001 -2.58723622781\n",
      "\n",
      "num = \n",
      "137\n",
      "SENSOR 2 1.44542795818 0.621521479992\n",
      "\n",
      "num = \n",
      "138\n",
      "SENSOR 3 4.16016807253 -0.216426375373\n",
      "\n",
      "num = \n",
      "139\n",
      "SENSOR 7 4.49513851102 -1.01028497886\n",
      "\n",
      "num = \n",
      "140\n",
      "ODOMETRY 0.000133544385368 0.100021589888 0.00109303965336\n",
      "\n",
      "sqP = \n",
      "[[0.35142462785845713, 0.0, 0.0], [0.02496900074051046, 0.2542361999421838, 0.0], [0.007668468022553291, 0.0009850662731417912, 0.2482814859818238]]\n",
      "sigma = \n",
      "[[1.42971432 1.04368266 2.73105881]\n",
      " [2.13256358 1.04368266 2.73105881]\n",
      " [1.47965232 1.55215506 2.73105881]\n",
      " [1.44505126 1.0456528  3.22762178]\n",
      " [0.72686506 1.04368266 2.73105881]\n",
      " [1.37977632 0.53521026 2.73105881]\n",
      " [1.41437738 1.04171253 2.23449584]]\n",
      "sqP = \n",
      "[[0.5693694103701459, 0.0, 0.0], [-0.008712257872654523, 0.5141362338990211, 0.0], [0.0061698845957660065, 0.0001307823447035413, 0.5116473205169912]]\n",
      "m = \n",
      "[1.42088234 1.11812678 2.84208112]\n",
      "P = \n",
      "[[0.12342002 0.00840109 0.00296509]\n",
      " [0.00840109 0.06414725 0.00098525]\n",
      " [0.00296509 0.00098525 0.06258626]]\n",
      "num = \n",
      "141\n",
      "SENSOR 8 4.07172862971 -1.54542949541\n",
      "\n",
      "num = \n",
      "142\n",
      "SENSOR 1 2.04327443852 -2.64266309511\n",
      "\n",
      "num = \n",
      "143\n",
      "SENSOR 2 1.62012370037 0.749575235679\n",
      "\n",
      "num = \n",
      "144\n",
      "SENSOR 3 4.2939412228 -0.211337985351\n",
      "\n",
      "num = \n",
      "145\n",
      "SENSOR 7 4.55765990908 -1.01012265001\n",
      "\n",
      "num = \n",
      "146\n",
      "ODOMETRY 0.000256984803676 0.100174110326 -7.36342307491e-05\n",
      "\n",
      "sqP = \n",
      "[[0.3513118604789895, 0.0, 0.0], [0.02391348407625718, 0.25214162087546876, 0.0], [0.008440057727069353, 0.0031070545060599216, 0.2500107358891358]]\n",
      "sigma = \n",
      "[[1.42088234 1.11812678 2.84208112]\n",
      " [2.12350606 1.11812678 2.84208112]\n",
      " [1.46870931 1.62241003 2.84208112]\n",
      " [1.43776245 1.12434089 3.34210259]\n",
      " [0.71825862 1.11812678 2.84208112]\n",
      " [1.37305537 0.61384354 2.84208112]\n",
      " [1.40400222 1.11191268 2.34205965]]\n",
      "sqP = \n",
      "[[0.5692654149061848, 0.0, 0.0], [-0.009194975297192525, 0.5131597419639453, 0.0], [0.006699713103532764, 0.0011311878527795031, 0.5124920523291955]]\n",
      "m = \n",
      "[1.41181999 1.04114543 2.85048174]\n",
      "P = \n",
      "[[0.12320777 0.00791482 0.00303456]\n",
      " [0.00791482 0.06310743 0.00131757]\n",
      " [0.00303456 0.00131757 0.06330018]]\n",
      "num = \n",
      "147\n",
      "SENSOR 8 4.06698092589 -1.56722255044\n",
      "\n",
      "num = \n",
      "148\n",
      "SENSOR 1 2.24512586768 -2.5437818487\n",
      "\n",
      "num = \n",
      "149\n",
      "SENSOR 2 1.37145664293 0.831882746959\n",
      "\n",
      "num = \n",
      "150\n",
      "SENSOR 3 4.22397677315 -0.235036227202\n",
      "\n",
      "num = \n",
      "151\n",
      "SENSOR 7 4.41926718082 -1.00471126329\n",
      "\n",
      "num = \n",
      "152\n",
      "ODOMETRY 0.00058192498751 0.0999391933134 -0.00023129046615\n",
      "\n",
      "sqP = \n",
      "[[0.3510096420907424, 0.0, 0.0], [0.022548723584291518, 0.25019788281011, 0.0], [0.008645219997231095, 0.004486972705058757, 0.25140665739616413]]\n",
      "sigma = \n",
      "[[1.41181999 1.04114543 2.85048174]\n",
      " [2.11383927 1.04114543 2.85048174]\n",
      " [1.45691744 1.5415412  2.85048174]\n",
      " [1.42911043 1.05011937 3.35329505]\n",
      " [0.7098007  1.04114543 2.85048174]\n",
      " [1.36672254 0.54074966 2.85048174]\n",
      " [1.39452955 1.03217148 2.34766842]]\n",
      "sqP = \n",
      "[[0.5690263206607867, 0.0, 0.0], [-0.009819716799398755, 0.5122769216919767, 0.0], [0.006989315226263586, 0.0018033219211222235, 0.5131769144097202]]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "m = \n",
      "[1.40255442 0.99787701 3.00013391]\n",
      "P = \n",
      "[[0.1232345  0.00796055 0.00332184]\n",
      " [0.00796055 0.06296796 0.00130371]\n",
      " [0.00332184 0.00130371 0.06374295]]\n",
      "num = \n",
      "153\n",
      "SENSOR 8 3.97991042562 -1.60690152056\n",
      "\n",
      "num = \n",
      "154\n",
      "SENSOR 1 2.35643859208 -2.67866850272\n",
      "\n",
      "num = \n",
      "155\n",
      "SENSOR 2 1.45749408492 0.830132685583\n",
      "\n",
      "num = \n",
      "156\n",
      "SENSOR 3 4.02815458891 -0.258831937496\n",
      "\n",
      "num = \n",
      "157\n",
      "SENSOR 7 4.42145324114 -1.10938279886\n",
      "\n",
      "num = \n",
      "158\n",
      "ODOMETRY 0.000696732375949 0.100156157706 -0.000584089998119\n",
      "\n",
      "sqP = \n",
      "[[0.3510477123692497, 0.0, 0.0], [0.022676541004405002, 0.24990746023434768, 0.0], [0.00946264631390041, 0.004358115642509584, 0.25225863600680265]]\n",
      "sigma = \n",
      "[[1.40255442 0.99787701 3.00013391]\n",
      " [2.10464985 0.99787701 3.00013391]\n",
      " [1.44790751 1.49769193 3.00013391]\n",
      " [1.42147972 1.00659324 3.50465119]\n",
      " [0.700459   0.99787701 3.00013391]\n",
      " [1.35720134 0.49806209 3.00013391]\n",
      " [1.38362913 0.98916078 2.49561664]]\n",
      "sqP = \n",
      "[[0.5690678901035182, 0.0, 0.0], [-0.009702036971433245, 0.5121281339123687, 0.0], [0.007534418793785712, 0.0017129918877960262, 0.5136000321905455]]\n",
      "m = \n",
      "[1.41584443 1.03435806 3.11500574]\n",
      "P = \n",
      "[[0.12323054 0.00776185 0.00384462]\n",
      " [0.00776185 0.06158332 0.00142495]\n",
      " [0.00384462 0.00142495 0.06536311]]\n",
      "num = \n",
      "159\n",
      "SENSOR 8 4.14607416122 -1.62652743084\n",
      "\n",
      "num = \n",
      "160\n",
      "SENSOR 1 2.4585060639 -2.62149672021\n",
      "\n",
      "num = \n",
      "161\n",
      "SENSOR 2 1.39609946002 0.827146029562\n",
      "\n",
      "num = \n",
      "162\n",
      "SENSOR 3 4.01022306014 -0.221408239995\n",
      "\n",
      "num = \n",
      "163\n",
      "SENSOR 7 4.26091719102 -1.12261197694\n",
      "\n",
      "num = \n",
      "164\n",
      "ODOMETRY 3.6215575313e-05 0.0999441653561 0.000171557215833\n",
      "\n",
      "sqP = \n",
      "[[0.35104207036814067, 0.0, 0.0], [0.022110871440036452, 0.24717287901933777, 0.0], [0.010952033687969797, 0.00478527318086434, 0.25538258028269395]]\n",
      "sigma = \n",
      "[[1.41584443 1.03435806 3.11500574]\n",
      " [2.11792857 1.03435806 3.11500574]\n",
      " [1.46006617 1.52870382 3.11500574]\n",
      " [1.43774849 1.04392861 3.6257709 ]\n",
      " [0.71376028 1.03435806 3.11500574]\n",
      " [1.37162268 0.54001231 3.11500574]\n",
      " [1.39394036 1.02478752 2.60424058]]\n",
      "sqP = \n",
      "[[0.5690688647660236, 0.0, 0.0], [-0.010123290468367687, 0.5108353557388318, 0.0], [0.007996662893420879, 0.0018936046915849975, 0.5151368825746044]]\n",
      "m = \n",
      "[1.42663148 1.01604934 3.21211206]\n",
      "P = \n",
      "[[0.12300802 0.00730747 0.00395245]\n",
      " [0.00730747 0.06033641 0.00147102]\n",
      " [0.00395245 0.00147102 0.06648561]]\n",
      "num = \n",
      "165\n",
      "SENSOR 8 4.09025633382 -1.62902600145\n",
      "\n",
      "num = \n",
      "166\n",
      "SENSOR 1 2.36969383929 -2.76059077539\n",
      "\n",
      "num = \n",
      "167\n",
      "SENSOR 2 1.19862098202 1.02746191761\n",
      "\n",
      "num = \n",
      "168\n",
      "SENSOR 3 3.79856589715 -0.166995804924\n",
      "\n",
      "num = \n",
      "169\n",
      "SENSOR 7 4.28290107013 -1.22394878975\n",
      "\n",
      "num = \n",
      "170\n",
      "ODOMETRY -8.01303873469e-05 0.100119970896 0.000927184717075\n",
      "\n",
      "sqP = \n",
      "[[0.35072499409922747, 0.0, 0.0], [0.02083534119355251, 0.24474945068214163, 0.0], [0.01126938017153708, 0.005050963275809497, 0.2575521254026447]]\n",
      "sigma = \n",
      "[[1.42663148 1.01604934 3.21211206]\n",
      " [2.12808147 1.01604934 3.21211206]\n",
      " [1.46830216 1.50554824 3.21211206]\n",
      " [1.44917024 1.02615127 3.72721631]\n",
      " [0.72518149 1.01604934 3.21211206]\n",
      " [1.3849608  0.52655044 3.21211206]\n",
      " [1.40409272 1.00594741 2.69700781]]\n",
      "sqP = \n",
      "[[0.5688313738341139, 0.0, 0.0], [-0.010709911267769198, 0.5097249441845269, 0.0], [0.00797249054878341, 0.0020391274069566333, 0.5162094433635778]]\n",
      "m = \n",
      "[1.39717263 1.01447881 3.31135715]\n",
      "P = \n",
      "[[0.12281903 0.00704323 0.00413832]\n",
      " [0.00704323 0.05932096 0.00134697]\n",
      " [0.00413832 0.00134697 0.06763155]]\n",
      "num = \n",
      "171\n",
      "SENSOR 8 4.07670842571 -1.65993610399\n",
      "\n",
      "num = \n",
      "172\n",
      "SENSOR 1 2.69624348373 -2.70745662616\n",
      "\n",
      "num = \n",
      "173\n",
      "SENSOR 2 1.27863825632 1.0228680341\n",
      "\n",
      "num = \n",
      "174\n",
      "SENSOR 3 3.69329884881 -0.310970841602\n",
      "\n",
      "num = \n",
      "175\n",
      "SENSOR 7 4.33458610704 -1.19229396722\n",
      "\n",
      "num = \n",
      "176\n",
      "ODOMETRY 0.000523456685193 0.100105137199 9.69165742264e-05\n",
      "\n",
      "sqP = \n",
      "[[0.3504554621317897, 0.0, 0.0], [0.020097369104387527, 0.2427283581834537, 0.0], [0.011808405142364026, 0.0045715974735192795, 0.2597522043021889]]\n",
      "sigma = \n",
      "[[1.39717263 1.01447881 3.31135715]\n",
      " [2.09808355 1.01447881 3.31135715]\n",
      " [1.43736737 1.49993553 3.31135715]\n",
      " [1.42078944 1.02362201 3.83086156]\n",
      " [0.6962617  1.01447881 3.31135715]\n",
      " [1.35697789 0.52902209 3.31135715]\n",
      " [1.37355582 1.00533562 2.79185275]]\n",
      "sqP = \n",
      "[[0.5686496053062294, 0.0, 0.0], [-0.011017966028120938, 0.5087864011402524, 0.0], [0.00884924166952904, 0.0017831326951870498, 0.5173279237861359]]\n",
      "m = \n",
      "[1.41060863 0.99697732 3.45398238]\n",
      "P = \n",
      "[[0.12256343 0.00674437 0.0042984 ]\n",
      " [0.00674437 0.05824244 0.00114233]\n",
      " [0.0042984  0.00114233 0.06867242]]\n",
      "num = \n",
      "177\n",
      "SENSOR 8 4.19218444921 -1.62918746421\n",
      "\n",
      "num = \n",
      "178\n",
      "SENSOR 1 2.56469838469 -2.73365039673\n",
      "\n",
      "num = \n",
      "179\n",
      "SENSOR 2 1.07742306684 1.02436797734\n",
      "\n",
      "num = \n",
      "180\n",
      "SENSOR 3 3.79502409217 -0.2864736654\n",
      "\n",
      "num = \n",
      "181\n",
      "SENSOR 7 4.23474402594 -1.08843934559\n",
      "\n",
      "num = \n",
      "182\n",
      "ODOMETRY 0.000760851469688 0.0999197195975 -0.000675084333497\n",
      "\n",
      "sqP = \n",
      "[[0.35009060102761863, 0.0, 0.0], [0.019264655732452056, 0.24056457928990224, 0.0], [0.012277952018615694, 0.003765303581207248, 0.26173935883884275]]\n",
      "sigma = \n",
      "[[1.41060863 0.99697732 3.45398238]\n",
      " [2.11078984 0.99697732 3.45398238]\n",
      " [1.44913794 1.47810648 3.45398238]\n",
      " [1.43516454 1.00450793 3.97746109]\n",
      " [0.71042743 0.99697732 3.45398238]\n",
      " [1.37207932 0.51584816 3.45398238]\n",
      " [1.38605273 0.98944672 2.93050366]]\n",
      "sqP = \n",
      "[[0.5684059324058296, 0.0, 0.0], [-0.011467758567839095, 0.5077899353806301, 0.0], [0.008826523753972002, 0.001370319631832483, 0.5183218037898562]]\n",
      "m = \n",
      "[1.37870089 0.9495547  3.44788076]\n",
      "P = \n",
      "[[0.12219041 0.00644103 0.00435203]\n",
      " [0.00644103 0.05672789 0.00065288]\n",
      " [0.00435203 0.00065288 0.07011087]]\n",
      "num = \n",
      "183\n",
      "SENSOR 8 3.97471624618 -1.67287700529\n",
      "\n",
      "num = \n",
      "184\n",
      "SENSOR 1 2.75006267624 -2.92186387986\n",
      "\n",
      "num = \n",
      "185\n",
      "SENSOR 2 1.07790545646 1.14460483582\n",
      "\n",
      "num = \n",
      "186\n",
      "SENSOR 3 3.65459543213 -0.30509704695\n",
      "\n",
      "num = \n",
      "187\n",
      "SENSOR 7 4.07939470262 -1.19849586955\n",
      "\n",
      "num = \n",
      "188\n",
      "ODOMETRY 0.0001776465883 0.100000660883 -0.000138153337743\n",
      "\n",
      "sqP = \n",
      "[[0.3495574507167928, 0.0, 0.0], [0.018426255531888036, 0.23746234328976645, 0.0], [0.012450126718478177, 0.0017833237922488074, 0.26448570246743924]]\n",
      "sigma = \n",
      "[[1.37870089 0.9495547  3.44788076]\n",
      " [2.07781579 0.9495547  3.44788076]\n",
      " [1.4155534  1.42447939 3.44788076]\n",
      " [1.40360114 0.95312135 3.97685216]\n",
      " [0.67958598 0.9495547  3.44788076]\n",
      " [1.34184837 0.47463001 3.44788076]\n",
      " [1.35380063 0.94598805 2.91890935]]\n",
      "sqP = \n",
      "[[0.5680536452658813, 0.0, 0.0], [-0.011767120380673074, 0.5063548731519892, 0.0], [0.0095757315851113, 0.0003360676127452408, 0.5197331842426121]]\n",
      "m = \n",
      "[1.36575001 1.03998501 3.57303441]\n",
      "P = \n",
      "[[0.12220566 0.00663731 0.0046679 ]\n",
      " [0.00663731 0.05682489 0.00050163]\n",
      " [0.0046679  0.00050163 0.0704753 ]]\n",
      "num = \n",
      "189\n",
      "SENSOR 8 4.04139703382 -1.80527111094\n",
      "\n",
      "num = \n",
      "190\n",
      "SENSOR 1 2.81682858649 -2.73778261664\n",
      "\n",
      "num = \n",
      "191\n",
      "SENSOR 2 1.01157732908 1.35009000631\n",
      "\n",
      "num = \n",
      "192\n",
      "SENSOR 3 3.43641149431 -0.177181332772\n",
      "\n",
      "num = \n",
      "193\n",
      "SENSOR 7 4.1134848859 -1.19890280774\n",
      "\n",
      "num = \n",
      "194\n",
      "ODOMETRY -9.54350459669e-05 0.100059958212 0.000505361195257\n",
      "\n",
      "sqP = \n",
      "[[0.34957925499696435, 0.0, 0.0], [0.018986578767968117, 0.23762238844217712, 0.0], [0.013352912443192116, 0.0010441262297259933, 0.2651337522174329]]\n",
      "sigma = \n",
      "[[1.36575001 1.03998501 3.57303441]\n",
      " [2.06490852 1.03998501 3.57303441]\n",
      " [1.40372317 1.51522978 3.57303441]\n",
      " [1.39245583 1.04207326 4.10330191]\n",
      " [0.6665915  1.03998501 3.57303441]\n",
      " [1.32777685 0.56474023 3.57303441]\n",
      " [1.33904418 1.03789675 3.04276691]]\n",
      "sqP = \n",
      "[[0.5681060164865889, 0.0, 0.0], [-0.01151369172219688, 0.5064079298212635, 0.0], [0.010279354485700117, -9.120113233198158e-05, 0.5200730516957907]]\n",
      "m = \n",
      "[1.35045634 1.03608147 3.70832595]\n",
      "P = \n",
      "[[0.12170248 0.00581495 0.00416783]\n",
      " [0.00581495 0.05531904 0.00029435]\n",
      " [0.00416783 0.00029435 0.07076363]]\n",
      "num = \n",
      "195\n",
      "SENSOR 8 4.14771152129 -1.65785363504\n",
      "\n",
      "num = \n",
      "196\n",
      "SENSOR 1 2.97373538756 -2.76749218654\n",
      "\n",
      "num = \n",
      "197\n",
      "SENSOR 2 1.08572526299 1.27905533571\n",
      "\n",
      "num = \n",
      "198\n",
      "SENSOR 3 3.19721928714 -0.285485802808\n",
      "\n",
      "num = \n",
      "199\n",
      "SENSOR 7 4.30425329678 -1.23670399341\n",
      "\n",
      "num = \n",
      "200\n",
      "ODOMETRY -0.000425877997443 0.100165010148 0.000263981771762\n",
      "\n",
      "sqP = \n",
      "[[0.3488588223801677, 0.0, 0.0], [0.01666850327741481, 0.23460860781682416, 0.0], [0.011947025579023982, 0.000405827994139235, 0.2657456128475276]]\n",
      "sigma = \n",
      "[[1.35045634 1.03608147 3.70832595]\n",
      " [2.04817398 1.03608147 3.70832595]\n",
      " [1.38379335 1.50529869 3.70832595]\n",
      " [1.37435039 1.03689313 4.23981717]\n",
      " [0.6527387  1.03608147 3.70832595]\n",
      " [1.31711933 0.56686425 3.70832595]\n",
      " [1.32656229 1.03526981 3.17683472]]\n",
      "sqP = \n",
      "[[0.5675588501417033, 0.0, 0.0], [-0.012452614250836365, 0.5050886106107549, 0.0], [0.009930083536387914, -0.0003807758883628109, 0.5203920334080299]]\n",
      "m = \n",
      "[1.35205086 0.97619523 3.85091954]\n",
      "P = \n",
      "[[ 0.12115468  0.00522095  0.00389718]\n",
      " [ 0.00522095  0.05391973 -0.00027859]\n",
      " [ 0.00389718 -0.00027859  0.0714908 ]]\n",
      "num = \n",
      "201\n",
      "SENSOR 8 4.0209837071 -1.67421338488\n",
      "\n",
      "num = \n",
      "202\n",
      "SENSOR 1 2.98678421634 -2.79971956357\n",
      "\n",
      "num = \n",
      "203\n",
      "SENSOR 2 1.01794738917 1.50929471669\n",
      "\n",
      "num = \n",
      "204\n",
      "SENSOR 3 3.17923741534 -0.348369090478\n",
      "\n",
      "num = \n",
      "205\n",
      "SENSOR 7 4.02065742023 -1.27095421415\n",
      "\n",
      "num = \n",
      "206\n",
      "ODOMETRY 6.98085102272e-05 0.0998571294686 0.000814819876331\n",
      "\n",
      "sqP = \n",
      "[[0.34807280922961426, 0.0, 0.0], [0.014999583071817497, 0.231721267659876, 0.0], [0.011196447895012186, -0.00192700597070224, 0.26713615383539946]]\n",
      "sigma = \n",
      "[[1.35205086 0.97619523 3.85091954]\n",
      " [2.04819648 0.97619523 3.85091954]\n",
      " [1.38205003 1.43963777 3.85091954]\n",
      " [1.37444376 0.97234122 4.38519185]\n",
      " [0.65590524 0.97619523 3.85091954]\n",
      " [1.3220517  0.5127527  3.85091954]\n",
      " [1.32965797 0.98004924 3.31664724]]\n",
      "sqP = \n",
      "[[0.5670141342707337, 0.0, 0.0], [-0.01321209201681944, 0.5038247982388079, 0.0], [0.009574894505330092, -0.001594847732296553, 0.5210983880698931]]\n",
      "m = \n",
      "[1.33477837 1.06178439 3.92924291]\n",
      "P = \n",
      "[[ 0.12075199  0.0059284   0.00361681]\n",
      " [ 0.0059284   0.05221707 -0.00157099]\n",
      " [ 0.00361681 -0.00157099  0.072579  ]]\n",
      "num = \n",
      "207\n",
      "SENSOR 8 3.94704456339 -1.84097040455\n",
      "\n",
      "num = \n",
      "208\n",
      "SENSOR 1 3.00070004218 -2.81153261862\n",
      "\n",
      "num = \n",
      "209\n",
      "SENSOR 2 1.11963656291 1.61697540263\n",
      "\n",
      "num = \n",
      "210\n",
      "SENSOR 3 3.16238211291 -0.347631273442\n",
      "\n",
      "num = \n",
      "211\n",
      "SENSOR 7 4.11202372197 -1.30111395385\n",
      "\n",
      "num = \n",
      "212\n",
      "ODOMETRY -4.66038294942e-05 0.100189586967 -5.21580102793e-05\n",
      "\n",
      "sqP = \n",
      "[[0.3474938657946447, 0.0, 0.0], [0.017060454298040206, 0.22787279093113555, 0.0], [0.010408258887152242, -0.007673381300971614, 0.2690943922053166]]\n",
      "sigma = \n",
      "[[1.33477837 1.06178439 3.92924291]\n",
      " [2.0297661  1.06178439 3.92924291]\n",
      " [1.36889928 1.51752997 3.92924291]\n",
      " [1.35559489 1.04643762 4.46743169]\n",
      " [0.63979064 1.06178439 3.92924291]\n",
      " [1.30065746 0.6060388  3.92924291]\n",
      " [1.31396186 1.07713115 3.39105412]]\n",
      "sqP = \n",
      "[[0.5667021949126578, 0.0, 0.0], [-0.01241150854993868, 0.5020630023211562, 0.0], [0.009541797149749892, -0.0046606269551782015, 0.5220926349092646]]\n",
      "m = \n",
      "[1.37213266 1.10449078 3.93888015]\n",
      "P = \n",
      "[[1.21833471e-01 8.06303207e-04 2.99547966e-03]\n",
      " [8.06303207e-04 5.27246267e-02 7.54823719e-05]\n",
      " [2.99547966e-03 7.54823719e-05 7.17808948e-02]]\n",
      "num = \n",
      "213\n",
      "SENSOR 8 4.01514296922 -1.96395215815\n",
      "\n",
      "num = \n",
      "214\n",
      "SENSOR 1 3.31125551661 -2.81890526713\n",
      "\n",
      "num = \n",
      "215\n",
      "SENSOR 2 1.08993866286 1.69673860324\n",
      "\n",
      "num = \n",
      "216\n",
      "SENSOR 3 3.16301966354 -0.29879355839\n",
      "\n",
      "num = \n",
      "217\n",
      "SENSOR 7 4.0697079487 -1.2813948975\n",
      "\n",
      "num = \n",
      "218\n",
      "ODOMETRY -1.1888042286e-05 0.0999378556007 0.000874157595222\n",
      "\n",
      "sqP = \n",
      "[[0.34904651676594906, 0.0, 0.0], [0.0023100164823216737, 0.22960681730459914, 0.0], [0.008581892442885446, 0.00024240595095191493, 0.2677819769648545]]\n",
      "sigma = \n",
      "[[1.37213266 1.10449078 3.93888015]\n",
      " [2.07022569 1.10449078 3.93888015]\n",
      " [1.37675269 1.56370441 3.93888015]\n",
      " [1.38929645 1.10497559 4.4744441 ]\n",
      " [0.67403963 1.10449078 3.93888015]\n",
      " [1.36751263 0.64527714 3.93888015]\n",
      " [1.35496888 1.10400597 3.40331619]]\n",
      "sqP = \n",
      "[[0.5673732950364264, 0.0, 0.0], [-0.018494017454988625, 0.5032632118070421, 0.0], [0.007963456794002613, -0.0003653114505444797, 0.5214152907002443]]\n",
      "m = \n",
      "[1.39912295 1.07870508 4.06123112]\n",
      "P = \n",
      "[[1.21776697e-01 4.31346855e-05 2.63774284e-03]\n",
      " [4.31346855e-05 5.31474161e-02 6.55992925e-05]\n",
      " [2.63774284e-03 6.55992925e-05 7.13635571e-02]]\n",
      "num = \n",
      "219\n",
      "SENSOR 8 4.2194758592 -1.87684781408\n",
      "\n",
      "num = \n",
      "220\n",
      "SENSOR 1 3.38916611019 -2.79022010215\n",
      "\n",
      "num = \n",
      "221\n",
      "SENSOR 2 0.953931009281 1.74716622466\n",
      "\n",
      "num = \n",
      "222\n",
      "SENSOR 3 2.97789976956 -0.385301687676\n",
      "\n",
      "num = \n",
      "223\n",
      "SENSOR 7 3.99090383462 -1.29098027348\n",
      "\n",
      "num = \n",
      "224\n",
      "ODOMETRY -0.000548398686482 0.0999074163285 -0.000586249282159\n",
      "\n",
      "sqP = \n",
      "[[0.3489651799109621, 0.0, 0.0], [0.00012360741980462587, 0.2305372005036465, 0.0], [0.007558756550095432, 0.0002804969173955325, 0.26703247678746395]]\n",
      "sigma = \n",
      "[[1.39912295 1.07870508 4.06123112]\n",
      " [2.09705331 1.07870508 4.06123112]\n",
      " [1.39937016 1.53977948 4.06123112]\n",
      " [1.41424046 1.07926608 4.59529607]\n",
      " [0.70119259 1.07870508 4.06123112]\n",
      " [1.39887573 0.61763068 4.06123112]\n",
      " [1.38400543 1.07814409 3.52716616]]\n",
      "sqP = \n",
      "[[0.5673040162643552, 0.0, 0.0], [-0.0194183893958203, 0.5037548829688302, 0.0], [0.006934295615126928, -0.00028805837272137676, 0.521014950065474]]\n",
      "m = \n",
      "[1.42353299 1.01561061 4.19642282]\n",
      "P = \n",
      "[[ 0.12315906 -0.00217465  0.00199169]\n",
      " [-0.00217465  0.05350802  0.00023287]\n",
      " [ 0.00199169  0.00023287  0.07136639]]\n",
      "num = \n",
      "225\n",
      "SENSOR 8 4.01544662734 -1.8776296021\n",
      "\n",
      "num = \n",
      "226\n",
      "SENSOR 1 3.58685644655 -2.88216804\n",
      "\n",
      "num = \n",
      "227\n",
      "SENSOR 2 1.09959994011 1.88380797133\n",
      "\n",
      "num = \n",
      "228\n",
      "SENSOR 3 3.10144999253 -0.263328469751\n",
      "\n",
      "num = \n",
      "229\n",
      "SENSOR 7 4.13471788936 -1.42326420591\n",
      "\n",
      "num = \n",
      "230\n",
      "ODOMETRY 0.000209771470498 0.0999383153862 0.000442898738476\n",
      "\n",
      "sqP = \n",
      "[[0.3509402544662354, 0.0, 0.0], [-0.006196629694042943, 0.23123498320436736, 0.0], [0.005675301438596781, 0.0011591439889588597, 0.26708208385032706]]\n",
      "sigma = \n",
      "[[1.42353299 1.01561061 4.19642282]\n",
      " [2.1254135  1.01561061 4.19642282]\n",
      " [1.41113973 1.47808058 4.19642282]\n",
      " [1.4348836  1.0179289  4.73058699]\n",
      " [0.72165249 1.01561061 4.19642282]\n",
      " [1.43592625 0.55314064 4.19642282]\n",
      " [1.41218239 1.01329232 3.66225865]]\n",
      "sqP = \n",
      "[[0.5685329097517664, 0.0, 0.0], [-0.022212746058738873, 0.5042619123801027, 0.0], [0.0055999918878503665, 0.0002514605714591879, 0.5210286502603557]]\n",
      "m = \n",
      "[1.33140997 1.05738388 4.26657388]\n",
      "P = \n",
      "[[ 0.12537003  0.00928651 -0.00127992]\n",
      " [ 0.00928651  0.04980505 -0.00449583]\n",
      " [-0.00127992 -0.00449583  0.07322745]]\n",
      "num = \n",
      "231\n",
      "SENSOR 8 4.11908889453 -1.84906255817\n",
      "\n",
      "num = \n",
      "232\n",
      "SENSOR 1 3.40262134444 -2.79577392772\n",
      "\n",
      "num = \n",
      "233\n",
      "SENSOR 2 0.98313589166 1.93949068611\n",
      "\n",
      "num = \n",
      "234\n",
      "SENSOR 3 2.76424553473 -0.353284737573\n",
      "\n",
      "num = \n",
      "235\n",
      "SENSOR 7 4.04514369826 -1.34914177025\n",
      "\n",
      "num = \n",
      "236\n",
      "ODOMETRY -0.000636488566826 0.0999395698257 0.000643908734646\n",
      "\n",
      "sqP = \n",
      "[[0.3540763063652055, 0.0, 0.0], [0.026227416076951976, 0.2216239458534681, 0.0], [-0.003614801777308596, -0.01985806327932088, 0.2698518831358532]]\n",
      "sigma = \n",
      "[[1.33140997 1.05738388 4.26657388]\n",
      " [2.03956258 1.05738388 4.26657388]\n",
      " [1.3838648  1.50063178 4.26657388]\n",
      " [1.32418036 1.01766776 4.80627764]\n",
      " [0.62325735 1.05738388 4.26657388]\n",
      " [1.27895513 0.61413599 4.26657388]\n",
      " [1.33863957 1.09710001 3.72687011]]\n",
      "sqP = \n",
      "[[0.5710262470983184, 0.0, 0.0], [-0.009393691939365852, 0.4992903324076654, 0.0], [0.0030951431731517823, -0.010646339158951534, 0.5223144571493384]]\n",
      "m = \n",
      "[1.31699533 1.03553497 4.32470452]\n",
      "P = \n",
      "[[ 0.08472379  0.00644298 -0.00978108]\n",
      " [ 0.00644298  0.05527616 -0.00186622]\n",
      " [-0.00978108 -0.00186622  0.07334581]]\n",
      "num = \n",
      "237\n",
      "SENSOR 8 4.26368708251 -1.95171561203\n",
      "\n",
      "num = \n",
      "238\n",
      "SENSOR 1 3.59611549468 -2.8643885436\n",
      "\n",
      "num = \n",
      "239\n",
      "SENSOR 2 1.25741341306 2.05874595093\n",
      "\n",
      "num = \n",
      "240\n",
      "SENSOR 3 2.62985116185 -0.340176489157\n",
      "\n",
      "num = \n",
      "241\n",
      "SENSOR 7 4.0715354283 -1.34078750934\n",
      "\n",
      "num = \n",
      "242\n",
      "ODOMETRY -0.000749506228593 0.0998919597135 0.000431194734685\n",
      "\n",
      "sqP = \n",
      "[[0.2910735060846645, 0.0, 0.0], [0.02213523542954768, 0.2340644951539308, 0.0], [-0.03360347262967199, -0.004795274627453125, 0.26868871674075995]]\n",
      "sigma = \n",
      "[[1.31699533 1.03553497 4.32470452]\n",
      " [1.89914234 1.03553497 4.32470452]\n",
      " [1.3612658  1.50366396 4.32470452]\n",
      " [1.24978839 1.02594442 4.86208195]\n",
      " [0.73484832 1.03553497 4.32470452]\n",
      " [1.27272486 0.56740598 4.32470452]\n",
      " [1.38420228 1.04512552 3.78732708]]\n",
      "sqP = \n",
      "[[0.535110220388953, 0.0, 0.0], [-0.004772626311966389, 0.5044587783089844, 0.0], [-0.013035267444666685, -0.0010293647013378145, 0.5212145143492993]]\n",
      "m = \n",
      "[1.38826483 1.07491475 4.49486242]\n",
      "P = \n",
      "[[ 0.09409861 -0.01052553 -0.00973675]\n",
      " [-0.01052553  0.05148363 -0.00315725]\n",
      " [-0.00973675 -0.00315725  0.07327558]]\n",
      "num = \n",
      "243\n",
      "SENSOR 8 4.21795338302 -1.92674399779\n",
      "\n",
      "num = \n",
      "244\n",
      "SENSOR 1 3.64715670833 -2.88849823757\n",
      "\n",
      "num = \n",
      "245\n",
      "SENSOR 2 1.15505103131 2.09289492551\n",
      "\n",
      "num = \n",
      "246\n",
      "SENSOR 3 2.52001295552 -0.472631064422\n",
      "\n",
      "num = \n",
      "247\n",
      "SENSOR 7 3.95528847465 -1.41585544885\n",
      "\n",
      "num = \n",
      "248\n",
      "ODOMETRY 0.000406907201319 0.0998586394002 4.54817335274e-05\n",
      "\n",
      "sqP = \n",
      "[[0.3067549624262861, 0.0, 0.0], [-0.034312499362165814, 0.2242906212434059, 0.0], [-0.031741135671206844, -0.018932456663379595, 0.26815972656551484]]\n",
      "sigma = \n",
      "[[1.38826483 1.07491475 4.49486242]\n",
      " [2.00177476 1.07491475 4.49486242]\n",
      " [1.31963983 1.523496   4.49486242]\n",
      " [1.32478256 1.03704984 5.03118187]\n",
      " [0.77475491 1.07491475 4.49486242]\n",
      " [1.45688983 0.62633351 4.49486242]\n",
      " [1.4517471  1.11277967 3.95854297]]\n",
      "sqP = \n",
      "[[0.5443192576781836, 0.0, 0.0], [-0.029362830378943355, 0.5020136325637801, 0.0], [-0.012610716330874067, -0.009724718510601969, 0.5209930119119306]]\n",
      "m = \n",
      "[1.37790414 1.06881478 4.57161281]\n",
      "P = \n",
      "[[ 0.07575027  0.00338604 -0.01155112]\n",
      " [ 0.00338604  0.05867822 -0.00216284]\n",
      " [-0.01155112 -0.00216284  0.07129186]]\n",
      "num = \n",
      "249\n",
      "SENSOR 8 4.36749740434 -2.0474823273\n",
      "\n",
      "num = \n",
      "250\n",
      "SENSOR 1 3.68174071666 -2.9201773312\n",
      "\n",
      "num = \n",
      "251\n",
      "SENSOR 2 1.22696609171 2.26296774635\n",
      "\n",
      "num = \n",
      "252\n",
      "SENSOR 3 2.41868212361 -0.336817224032\n",
      "\n",
      "num = \n",
      "253\n",
      "SENSOR 7 3.98911047334 -1.47028485405\n",
      "\n",
      "num = \n",
      "254\n",
      "ODOMETRY 0.00020261199028 0.0999232742711 -0.00079340859659\n",
      "\n",
      "sqP = \n",
      "[[0.2752276764249121, 0.0, 0.0], [0.012302703830695772, 0.24192326467093067, 0.0], [-0.04196933101741592, -0.006805909803566752, 0.26359839405169194]]\n",
      "sigma = \n",
      "[[1.37790414 1.06881478 4.57161281]\n",
      " [1.92835949 1.06881478 4.57161281]\n",
      " [1.40250955 1.55266131 4.57161281]\n",
      " [1.29396548 1.05520296 5.09880959]\n",
      " [0.82744879 1.06881478 4.57161281]\n",
      " [1.35329873 0.58496825 4.57161281]\n",
      " [1.4618428  1.0824266  4.04441602]]\n",
      "sqP = \n",
      "[[0.526937430002701, 0.0, 0.0], [-0.007554296143151717, 0.508481796122832, 0.0], [-0.01831617789004853, -0.0017918855159046693, 0.5184472472869668]]\n",
      "m = \n",
      "[1.45897502 1.09077209 4.66660074]\n",
      "P = \n",
      "[[ 0.09264802 -0.01143737 -0.00972576]\n",
      " [-0.01143737  0.053166   -0.00343295]\n",
      " [-0.00972576 -0.00343295  0.07052835]]\n",
      "num = \n",
      "255\n",
      "SENSOR 8 4.3737723756 -1.93110738924\n",
      "\n",
      "num = \n",
      "256\n",
      "SENSOR 1 3.8479658409 -2.96461382002\n",
      "\n",
      "num = \n",
      "257\n",
      "SENSOR 2 1.44650342879 2.19184285471\n",
      "\n",
      "num = \n",
      "258\n",
      "SENSOR 3 2.4378797797 -0.376196945898\n",
      "\n",
      "num = \n",
      "259\n",
      "SENSOR 7 3.99144004155 -1.53192738274\n",
      "\n",
      "num = \n",
      "260\n",
      "ODOMETRY 0.000476456640183 0.0999946391606 0.000673997332114\n",
      "\n",
      "sqP = \n",
      "[[0.3043813727292534, 0.0, 0.0], [-0.03757577250605855, 0.22749518776676414, 0.0], [-0.0319525538214373, -0.020367852366157117, 0.26285458823740754]]\n",
      "sigma = \n",
      "[[1.45897502 1.09077209 4.66660074]\n",
      " [2.06773777 1.09077209 4.66660074]\n",
      " [1.38382348 1.54576246 4.66660074]\n",
      " [1.39506992 1.05003638 5.19230992]\n",
      " [0.85021228 1.09077209 4.66660074]\n",
      " [1.53412657 0.63578171 4.66660074]\n",
      " [1.52288013 1.13150779 4.14089156]]\n",
      "sqP = \n",
      "[[0.543213516436846, 0.0, 0.0], [-0.030892395779938072, 0.5035994157216249, 0.0], [-0.013628441822592804, -0.01015814319115455, 0.5183491219153972]]\n",
      "m = \n",
      "[1.43625612 1.07150657 4.76561285]\n",
      "P = \n",
      "[[ 0.0750719   0.00318569 -0.01112908]\n",
      " [ 0.00318569  0.06060696 -0.00233556]\n",
      " [-0.01112908 -0.00233556  0.06917862]]\n",
      "num = \n",
      "261\n",
      "SENSOR 8 4.18501121114 -1.96001604794\n",
      "\n",
      "num = \n",
      "262\n",
      "SENSOR 1 3.94177735119 -2.84551297654\n",
      "\n",
      "num = \n",
      "263\n",
      "SENSOR 2 1.18038634345 2.41547551073\n",
      "\n",
      "num = \n",
      "264\n",
      "SENSOR 3 2.25386927139 -0.458608006904\n",
      "\n",
      "num = \n",
      "265\n",
      "SENSOR 7 4.01204561994 -1.48975256344\n",
      "\n",
      "num = \n",
      "266\n",
      "ODOMETRY 0.000136092271998 0.0999996467097 -0.000522848477433\n",
      "\n",
      "sqP = \n",
      "[[0.27399251769270666, 0.0, 0.0], [0.011626927677443617, 0.245910102550982, 0.0], [-0.040618190345229635, -0.007577125164625735, 0.2597525227044474]]\n",
      "sigma = \n",
      "[[1.43625612 1.07150657 4.76561285]\n",
      " [1.98424116 1.07150657 4.76561285]\n",
      " [1.45950998 1.56332677 4.76561285]\n",
      " [1.35501974 1.05635232 5.28511789]\n",
      " [0.88827109 1.07150657 4.76561285]\n",
      " [1.41300227 0.57968636 4.76561285]\n",
      " [1.5174925  1.08666082 4.2461078 ]]\n",
      "sqP = \n",
      "[[0.5261719515383977, 0.0, 0.0], [-0.007758704906360897, 0.5104174431933699, 0.0], [-0.01819355095343283, -0.0021739246942286895, 0.5166287016861928]]\n",
      "m = \n",
      "[1.51604762 1.12592265 4.81090038]\n",
      "P = \n",
      "[[ 0.09309491 -0.01185682 -0.00949173]\n",
      " [-0.01185682  0.0544322  -0.00329866]\n",
      " [-0.00949173 -0.00329866  0.0680815 ]]\n",
      "num = \n",
      "267\n",
      "SENSOR 8 4.40946468491 -2.02094252774\n",
      "\n",
      "num = \n",
      "268\n",
      "SENSOR 1 3.91922179189 -2.87072202395\n",
      "\n",
      "num = \n",
      "269\n",
      "SENSOR 2 1.44580310453 2.32508479098\n",
      "\n",
      "num = \n",
      "270\n",
      "SENSOR 3 2.15756119315 -0.424067573571\n",
      "\n",
      "num = \n",
      "271\n",
      "SENSOR 7 4.06104474419 -1.57224455589\n",
      "\n",
      "num = \n",
      "272\n",
      "ODOMETRY 0.000302269878199 0.0999353432319 0.000539895064198\n",
      "\n",
      "sqP = \n",
      "[[0.30511457918961454, 0.0, 0.0], [-0.038860207245690906, 0.23004799809102433, 0.0], [-0.031108744336197282, -0.019593954927522174, 0.2583211571596922]]\n",
      "sigma = \n",
      "[[1.51604762 1.12592265 4.81090038]\n",
      " [2.12627678 1.12592265 4.81090038]\n",
      " [1.43832721 1.58601865 4.81090038]\n",
      " [1.45383013 1.08673474 5.3275427 ]\n",
      " [0.90581847 1.12592265 4.81090038]\n",
      " [1.59376804 0.66582665 4.81090038]\n",
      " [1.57826511 1.16511056 4.29425807]]\n",
      "sqP = \n",
      "[[0.5436660520166847, 0.0, 0.0], [-0.03183329762206187, 0.5047750401185748, 0.0], [-0.013878608012618528, -0.009488777562761948, 0.5161606444112801]]\n",
      "m = \n",
      "[1.52765417 1.06465661 4.91691933]\n",
      "P = \n",
      "[[ 0.07484815  0.00286943 -0.01056379]\n",
      " [ 0.00286943  0.06210553 -0.00222021]\n",
      " [-0.01056379 -0.00222021  0.06715743]]\n",
      "num = \n",
      "273\n",
      "SENSOR 8 4.5165550898 -2.00275541219\n",
      "\n",
      "num = \n",
      "274\n",
      "SENSOR 1 4.25083690758 -2.88476512783\n",
      "\n",
      "num = \n",
      "275\n",
      "SENSOR 2 1.59664742666 2.36949118143\n",
      "\n",
      "num = \n",
      "276\n",
      "SENSOR 3 2.18656915806 -0.37303565508\n",
      "\n",
      "num = \n",
      "277\n",
      "SENSOR 7 4.05007373159 -1.58049943139\n",
      "\n",
      "num = \n",
      "278\n",
      "ODOMETRY 0.00013361527452 0.100074513275 0.000560546767954\n",
      "\n",
      "sqP = \n",
      "[[0.2735838907609424, 0.0, 0.0], [0.010488317373300199, 0.24898901270733761, 0.0], [-0.03861260551904635, -0.007290410545965713, 0.25615102778447796]]\n",
      "sigma = \n",
      "[[1.52765417 1.06465661 4.91691933]\n",
      " [2.07482195 1.06465661 4.91691933]\n",
      " [1.54863081 1.56263464 4.91691933]\n",
      " [1.45042896 1.05007579 5.42922139]\n",
      " [0.98048639 1.06465661 4.91691933]\n",
      " [1.50667754 0.56667859 4.91691933]\n",
      " [1.60487938 1.07923744 4.40461728]]\n",
      "sqP = \n",
      "[[0.5257842555556298, 0.0, 0.0], [-0.008316097302741823, 0.5119497197052318, 0.0], [-0.01821891179479242, -0.002046987702212511, 0.5150079575152176]]\n",
      "m = \n",
      "[1.5118644  1.02593046 5.08323274]\n",
      "P = \n",
      "[[ 0.07111008  0.00708723 -0.01041905]\n",
      " [ 0.00708723  0.06279327 -0.00224267]\n",
      " [-0.01041905 -0.00224267  0.06618101]]\n",
      "num = \n",
      "279\n",
      "SENSOR 8 4.4359853347 -1.98886105814\n",
      "\n",
      "num = \n",
      "280\n",
      "SENSOR 1 4.34929216262 -2.84640872927\n",
      "\n",
      "num = \n",
      "281\n",
      "SENSOR 2 1.51357200054 2.46096763847\n",
      "\n",
      "num = \n",
      "282\n",
      "SENSOR 3 2.1177123351 -0.433574921219\n",
      "\n",
      "num = \n",
      "283\n",
      "SENSOR 7 3.63185364294 -1.65552297298\n",
      "\n",
      "num = \n",
      "284\n",
      "ODOMETRY -0.000748347401324 0.0999214044639 0.000183054107003\n",
      "\n",
      "sqP = \n",
      "[[0.2666647274891491, 0.0, 0.0], [0.02657732135683654, 0.24917245392204052, 0.0], [-0.03907170385872872, -0.004832995296290857, 0.2542263895367602]]\n",
      "sigma = \n",
      "[[1.5118644  1.02593046 5.08323274]\n",
      " [2.04519386 1.02593046 5.08323274]\n",
      " [1.56501905 1.52427537 5.08323274]\n",
      " [1.433721   1.01626447 5.59168552]\n",
      " [0.97853495 1.02593046 5.08323274]\n",
      " [1.45870976 0.52758556 5.08323274]\n",
      " [1.59000781 1.03559645 4.57477996]]\n",
      "sqP = \n",
      "[[0.5228221771690552, 0.0, 0.0], [-0.0003513726109410073, 0.5112897397463436, 0.0], [-0.018211348277446103, -0.00047118627045805413, 0.5140234620490757]]\n",
      "m = \n",
      "[1.55411458 1.20874408 5.13876636]\n",
      "P = \n",
      "[[ 0.09283325 -0.01142116 -0.00948223]\n",
      " [-0.01142116  0.05628957 -0.00252277]\n",
      " [-0.00948223 -0.00252277  0.06447761]]\n",
      "num = \n",
      "285\n",
      "SENSOR 8 4.33976493299 -2.0786008695\n",
      "\n",
      "num = \n",
      "286\n",
      "SENSOR 1 4.26489841524 -2.98062522043\n",
      "\n",
      "num = \n",
      "287\n",
      "SENSOR 2 1.61966127202 2.46647468873\n",
      "\n",
      "num = \n",
      "288\n",
      "SENSOR 3 2.02261768412 -0.484039818089\n",
      "\n",
      "num = \n",
      "289\n",
      "SENSOR 7 4.09245647518 -1.63859759267\n",
      "\n",
      "num = \n",
      "290\n",
      "ODOMETRY 0.000428845906387 0.10008081915 0.000352531758719\n",
      "\n",
      "sqP = \n",
      "[[0.3046854894571432, 0.0, 0.0], [-0.037485093964758597, 0.2342742878969535, 0.0], [-0.03112136886523474, -0.01574801906165881, 0.2515175262805732]]\n",
      "sigma = \n",
      "[[1.55411458 1.20874408 5.13876636]\n",
      " [2.16348556 1.20874408 5.13876636]\n",
      " [1.4791444  1.67729266 5.13876636]\n",
      " [1.49187185 1.17724804 5.64180141]\n",
      " [0.94474361 1.20874408 5.13876636]\n",
      " [1.62908477 0.74019551 5.13876636]\n",
      " [1.61635732 1.24024012 4.63573131]]\n",
      "sqP = \n",
      "[[0.5433294758756713, 0.0, 0.0], [-0.03172997734712503, 0.5065786779651821, 0.0], [-0.014119585635577775, -0.007215954201513814, 0.512876420024668]]\n",
      "m = \n",
      "[1.55713544 1.11342106 5.16514675]\n",
      "P = \n",
      "[[ 0.06630161  0.00285905 -0.00818591]\n",
      " [ 0.00285905  0.06531473 -0.00119296]\n",
      " [-0.00818591 -0.00119296  0.06301892]]\n",
      "num = \n",
      "291\n",
      "SENSOR 8 4.66591979904 -2.11042811091\n",
      "\n",
      "num = \n",
      "292\n",
      "SENSOR 1 4.5400120979 -2.94675157545\n",
      "\n",
      "num = \n",
      "293\n",
      "SENSOR 2 1.66151980155 2.4772319982\n",
      "\n",
      "num = \n",
      "294\n",
      "SENSOR 3 1.69406105599 -0.528339655367\n",
      "\n",
      "num = \n",
      "295\n",
      "SENSOR 7 3.93440624897 -1.69607534156\n",
      "\n",
      "num = \n",
      "296\n",
      "ODOMETRY 0.000241280703103 0.100190717382 0.00021908908565\n",
      "\n",
      "sqP = \n",
      "[[0.25749099002506765, 0.0, 0.0], [0.011103482715605548, 0.25532614543689236, 0.0], [-0.03179107251436974, -0.003289803579022604, 0.2489928215962209]]\n",
      "sigma = \n",
      "[[1.55713544 1.11342106 5.16514675]\n",
      " [2.07211742 1.11342106 5.16514675]\n",
      " [1.57934241 1.62407335 5.16514675]\n",
      " [1.4935533  1.10684145 5.66313239]\n",
      " [1.04215346 1.11342106 5.16514675]\n",
      " [1.53492848 0.60276877 5.16514675]\n",
      " [1.62071759 1.12000067 4.6671611 ]]\n",
      "sqP = \n",
      "[[0.5171417306349577, 0.0, 0.0], [-0.00680537663042785, 0.5149624528219783, 0.0], [-0.015141456294471237, -0.00026108731032284074, 0.5116429187896758]]\n",
      "m = \n",
      "[1.52930115 1.02536475 5.40568228]\n",
      "P = \n",
      "[[ 0.09283126 -0.01031079 -0.00765472]\n",
      " [-0.01031079  0.05853687 -0.00097573]\n",
      " [-0.00765472 -0.00097573  0.06302717]]\n",
      "num = \n",
      "297\n",
      "SENSOR 8 4.65394245626 -2.10046665109\n",
      "\n",
      "num = \n",
      "298\n",
      "SENSOR 1 4.6922590945 -2.8776113437\n",
      "\n",
      "num = \n",
      "299\n",
      "SENSOR 2 1.84720641205 2.51332375216\n",
      "\n",
      "num = \n",
      "300\n",
      "SENSOR 3 1.7878592279 -0.6464169453\n",
      "\n",
      "num = \n",
      "301\n",
      "SENSOR 7 4.08108369627 -1.67639330679\n",
      "\n",
      "num = \n",
      "302\n",
      "ODOMETRY -0.00119608488614 0.0998300419662 -0.000785692437084\n",
      "\n",
      "sqP = \n",
      "[[0.30468223260362004, 0.0, 0.0], [-0.03384112725227462, 0.23956553242932344, 0.0], [-0.025123613395785945, -0.0076218776130372, 0.2496755572392562]]\n",
      "sigma = \n",
      "[[1.52930115 1.02536475 5.40568228]\n",
      " [2.13866562 1.02536475 5.40568228]\n",
      " [1.4616189  1.50449581 5.40568228]\n",
      " [1.47905393 1.01012099 5.90503339]\n",
      " [0.91993669 1.02536475 5.40568228]\n",
      " [1.59698341 0.54623368 5.40568228]\n",
      " [1.57954838 1.0406085  4.90633116]]\n",
      "sqP = \n",
      "[[0.5427777452218283, 0.0, 0.0], [-0.030992021239207555, 0.5088333473517704, 0.0], [-0.010879466473769528, -0.0032963897180572875, 0.5120725972127101]]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "m = \n",
      "[1.51083417 1.03117063 5.52115757]\n",
      "P = \n",
      "[[ 0.06601219  0.00321472 -0.00693927]\n",
      " [ 0.00321472  0.06730097  0.00019165]\n",
      " [-0.00693927  0.00019165  0.0616711 ]]\n",
      "num = \n",
      "303\n",
      "SENSOR 8 4.77404605226 -2.08170107067\n",
      "\n",
      "num = \n",
      "304\n",
      "SENSOR 1 4.33976416105 -2.90292850512\n",
      "\n",
      "num = \n",
      "305\n",
      "SENSOR 2 1.88969783614 2.58825155721\n",
      "\n",
      "num = \n",
      "306\n",
      "SENSOR 3 1.73764363829 -0.49745195748\n",
      "\n",
      "num = \n",
      "307\n",
      "SENSOR 7 3.96406958388 -1.66060095497\n",
      "\n",
      "num = \n",
      "308\n",
      "ODOMETRY -0.000283673180471 0.0998692948249 -6.45801742076e-05\n",
      "\n",
      "sqP = \n",
      "[[0.25692837703208715, 0.0, 0.0], [0.012512106276293355, 0.2591223948749776, 0.0], [-0.02700856957007688, 0.002043765036733196, 0.24685513974011622]]\n",
      "sigma = \n",
      "[[1.51083417 1.03117063 5.52115757]\n",
      " [2.02469092 1.03117063 5.52115757]\n",
      " [1.53585838 1.54941542 5.52115757]\n",
      " [1.45681703 1.03525816 6.01486785]\n",
      " [0.99697741 1.03117063 5.52115757]\n",
      " [1.48580996 0.51292584 5.52115757]\n",
      " [1.56485131 1.0270831  5.02744729]]\n",
      "sqP = \n",
      "[[0.5166219183871638, 0.0, 0.0], [-0.006181089960298631, 0.516801664877873, 0.0], [-0.012149139668832909, 0.0020849476904760415, 0.5106243629949457]]\n",
      "m = \n",
      "[1.54126914 1.14110768 5.48137239]\n",
      "P = \n",
      "[[ 0.09244969 -0.01220316 -0.00886533]\n",
      " [-0.01220316  0.05829137  0.00014377]\n",
      " [-0.00886533  0.00014377  0.06095644]]\n",
      "num = \n",
      "309\n",
      "SENSOR 8 4.72928958355 -2.11808295378\n",
      "\n",
      "num = \n",
      "310\n",
      "SENSOR 1 4.85446874563 -2.96828803681\n",
      "\n",
      "num = \n",
      "311\n",
      "SENSOR 2 1.94629939838 2.61814643875\n",
      "\n",
      "num = \n",
      "312\n",
      "SENSOR 3 1.60770645745 -0.563753684955\n",
      "\n",
      "num = \n",
      "313\n",
      "SENSOR 7 3.89182668135 -1.78660920606\n",
      "\n",
      "num = \n",
      "314\n",
      "ODOMETRY -0.000182066224786 0.0998742815469 -0.000565724685502\n",
      "\n",
      "sqP = \n",
      "[[0.3040553995125991, 0.0, 0.0], [-0.04013464579308943, 0.2380768444518029, 0.0], [-0.029156948990486244, -0.004311365078202237, 0.24512797239013842]]\n",
      "sigma = \n",
      "[[1.54126914 1.14110768 5.48137239]\n",
      " [2.14937994 1.14110768 5.48137239]\n",
      " [1.46099985 1.61726137 5.48137239]\n",
      " [1.48295524 1.13248495 5.97162833]\n",
      " [0.93315834 1.14110768 5.48137239]\n",
      " [1.62153843 0.66495399 5.48137239]\n",
      " [1.59958304 1.14973041 4.99111644]]\n",
      "sqP = \n",
      "[[0.5430567221203848, 0.0, 0.0], [-0.03378150367502421, 0.5082088332270082, 0.0], [-0.012671449645700168, -0.0016195947234980978, 0.5098447998561998]]\n",
      "m = \n",
      "[1.53465876 1.13826663 5.656852  ]\n",
      "P = \n",
      "[[ 0.06575838  0.0024102  -0.00706879]\n",
      " [ 0.0024102   0.06825802  0.00062209]\n",
      " [-0.00706879  0.00062209  0.06057545]]\n",
      "num = \n",
      "315\n",
      "SENSOR 8 4.66229866868 -2.20515623143\n",
      "\n",
      "num = \n",
      "316\n",
      "SENSOR 1 4.80809797253 -3.00294348241\n",
      "\n",
      "num = \n",
      "317\n",
      "SENSOR 2 2.13430608854 2.71645106462\n",
      "\n",
      "num = \n",
      "318\n",
      "SENSOR 3 1.376952877 -0.663110570535\n",
      "\n",
      "num = \n",
      "319\n",
      "SENSOR 7 3.8811618647 -1.8038777696\n",
      "\n",
      "num = \n",
      "320\n",
      "ODOMETRY 0.000446460674214 0.0998339319617 0.000172792242606\n",
      "\n",
      "sqP = \n",
      "[[0.25643397770070897, 0.0, 0.0], [0.00939890428807147, 0.2610932448284972, 0.0], [-0.027565724874298512, 0.0033749662569533406, 0.24454894670135588]]\n",
      "sigma = \n",
      "[[1.53465876 1.13826663 5.656852  ]\n",
      " [2.04752672 1.13826663 5.656852  ]\n",
      " [1.55345657 1.66045312 5.656852  ]\n",
      " [1.47952732 1.14501656 6.14594989]\n",
      " [1.02179081 1.13826663 5.656852  ]\n",
      " [1.51586096 0.61608014 5.656852  ]\n",
      " [1.58979021 1.1315167  5.16775411]]\n",
      "sqP = \n",
      "[[0.5163396106334178, 0.0, 0.0], [-0.007752528801967222, 0.517936091486263, 0.0], [-0.012607851255860584, 0.0026792263866150154, 0.509529816990104]]\n",
      "m = \n",
      "[1.54221459 1.23801993 5.77130403]\n",
      "P = \n",
      "[[ 0.05965374  0.00722951 -0.0059615 ]\n",
      " [ 0.00722951  0.06923965  0.00162984]\n",
      " [-0.0059615   0.00162984  0.05989023]]\n",
      "num = \n",
      "321\n",
      "SENSOR 8 5.02546804585 -2.19510092046\n",
      "\n",
      "num = \n",
      "322\n",
      "SENSOR 1 5.01349268035 -2.96140277102\n",
      "\n",
      "num = \n",
      "323\n",
      "SENSOR 2 2.16725029068 2.71920290634\n",
      "\n",
      "num = \n",
      "324\n",
      "SENSOR 3 1.6217305118 -0.701135122275\n",
      "\n",
      "num = \n",
      "325\n",
      "SENSOR 7 3.94079630166 -1.73582055105\n",
      "\n",
      "num = \n",
      "326\n",
      "ODOMETRY -0.000192785040713 0.099962525761 -0.000198322747385\n",
      "\n",
      "sqP = \n",
      "[[0.2442411415791502, 0.0, 0.0], [0.02959988239609818, 0.26146414599673423, 0.0], [-0.0244082729590963, 0.008996728642899085, 0.2433383030859496]]\n",
      "sigma = \n",
      "[[1.54221459 1.23801993 5.77130403]\n",
      " [2.03069687 1.23801993 5.77130403]\n",
      " [1.60141435 1.76094822 5.77130403]\n",
      " [1.49339804 1.25601339 6.25798064]\n",
      " [1.05373231 1.23801993 5.77130403]\n",
      " [1.48301482 0.71509164 5.77130403]\n",
      " [1.59103114 1.22002647 5.28462742]]\n",
      "sqP = \n",
      "[[0.5110045519039874, 0.0, 0.0], [0.0032362344905374693, 0.5172010997123131, 0.0], [-0.011300061983135355, 0.005460698034596508, 0.5089678133536978]]\n",
      "m = \n",
      "[1.51441792 1.05552334 5.87021698]\n",
      "P = \n",
      "[[ 0.06256666 -0.00141972 -0.00537394]\n",
      " [-0.00141972  0.05958395  0.00233681]\n",
      " [-0.00537394  0.00233681  0.0589771 ]]\n",
      "num = \n",
      "327\n",
      "SENSOR 8 4.94083414559 -2.11537295171\n",
      "\n",
      "num = \n",
      "328\n",
      "SENSOR 9 4.91446231227 -0.887651756022\n",
      "\n",
      "num = \n",
      "329\n",
      "SENSOR 2 2.16197834316 2.69904984308\n",
      "\n",
      "num = \n",
      "330\n",
      "SENSOR 3 1.525427237 -0.711047076592\n",
      "\n",
      "num = \n",
      "331\n",
      "SENSOR 7 4.01530297276 -1.90341160103\n",
      "\n",
      "num = \n",
      "332\n",
      "ODOMETRY -0.00041677675012 0.0998935259811 -0.000185627166787\n",
      "\n",
      "sqP = \n",
      "[[0.2501332823791003, 0.0, 0.0], [-0.005675873622822524, 0.2440322451143084, 0.0], [-0.02148431063457274, 0.009076116097018666, 0.24172948654211276]]\n",
      "sigma = \n",
      "[[1.51441792 1.05552334 5.87021698]\n",
      " [2.01468449 1.05552334 5.87021698]\n",
      " [1.50306618 1.54358783 5.87021698]\n",
      " [1.4714493  1.07367557 6.35367595]\n",
      " [1.01415136 1.05552334 5.87021698]\n",
      " [1.52576967 0.56745885 5.87021698]\n",
      " [1.55738654 1.03737111 5.38675801]]\n",
      "sqP = \n",
      "[[0.5128941899637467, 0.0, 0.0], [-0.014849956219579402, 0.5102027903906838, 0.0], [-0.009459148340262008, 0.004961905002986735, 0.5082225017960298]]\n",
      "m = \n",
      "[1.57831059 1.14733475 5.92143134]\n",
      "P = \n",
      "[[ 0.09222374 -0.00960384 -0.00584141]\n",
      " [-0.00960384  0.05060813 -0.008197  ]\n",
      " [-0.00584141 -0.008197    0.07399382]]\n",
      "num = \n",
      "333\n",
      "SENSOR 8 4.9944171849 -2.22316600512\n",
      "\n",
      "num = \n",
      "334\n",
      "SENSOR 9 4.91770926354 -0.875068268808\n",
      "\n",
      "num = \n",
      "335\n",
      "SENSOR 2 2.37108525182 2.63893274328\n",
      "\n",
      "num = \n",
      "336\n",
      "SENSOR 3 1.22148996766 -0.779993952297\n",
      "\n",
      "num = \n",
      "337\n",
      "SENSOR 7 4.05942517061 -1.80602776182\n",
      "\n",
      "num = \n",
      "338\n",
      "ODOMETRY -0.000504187302557 0.099935062112 -0.000102208043102\n",
      "\n",
      "sqP = \n",
      "[[0.3036836218194307, 0.0, 0.0], [-0.031624485882115964, 0.22272858178795926, 0.0], [-0.019235192139202075, -0.03953376503060952, 0.26844162865875254]]\n",
      "sigma = \n",
      "[[1.57831059 1.14733475 5.92143134]\n",
      " [2.18567783 1.14733475 5.92143134]\n",
      " [1.51506162 1.59279191 5.92143134]\n",
      " [1.5398402  1.06826722 6.4583146 ]\n",
      " [0.97094334 1.14733475 5.92143134]\n",
      " [1.64155956 0.70187758 5.92143134]\n",
      " [1.61678097 1.22640228 5.38454809]]\n",
      "sqP = \n",
      "[[0.5418430057547178, 0.0, 0.0], [-0.027824119001686652, 0.5024711814525429, 0.0], [-0.009644742012485193, -0.02060875866532827, 0.5211619387775586]]\n",
      "m = \n",
      "[1.57417857 1.13788543 6.08752128]\n",
      "P = \n",
      "[[ 0.0737774   0.0032007  -0.00873285]\n",
      " [ 0.0032007   0.05979044 -0.00969354]\n",
      " [-0.00873285 -0.00969354  0.07610759]]\n",
      "num = \n",
      "339\n",
      "SENSOR 9 5.05113586384 -0.939362710755\n",
      "\n",
      "num = \n",
      "340\n",
      "SENSOR 2 2.42075573102 2.65401889272\n",
      "\n",
      "num = \n",
      "341\n",
      "SENSOR 3 1.40673857311 -0.792218436955\n",
      "\n",
      "num = \n",
      "342\n",
      "SENSOR 7 4.01097220078 -1.86268980877\n",
      "\n",
      "num = \n",
      "343\n",
      "ODOMETRY 0.00010208423308 0.10009595201 0.000112598536447\n",
      "\n",
      "sqP = \n",
      "[[0.271619953706921, 0.0, 0.0], [0.01178372565415955, 0.2442367307374026, 0.0], [-0.03215099985542614, -0.03813793452840578, 0.27132894847284195]]\n",
      "sigma = \n",
      "[[1.57417857 1.13788543 6.08752128]\n",
      " [2.11741848 1.13788543 6.08752128]\n",
      " [1.59774602 1.62635889 6.08752128]\n",
      " [1.50987657 1.06160956 6.63017918]\n",
      " [1.03093867 1.13788543 6.08752128]\n",
      " [1.55061112 0.64941197 6.08752128]\n",
      " [1.63848057 1.2141613  5.54486339]]\n",
      "sqP = \n",
      "[[0.5243566937054833, 0.0, 0.0], [-0.005775666195721978, 0.5108145096718079, 0.0], [-0.01667571994231223, -0.018739477562204697, 0.5225261385346973]]\n",
      "m = \n",
      "[1.65454432 1.0974784  6.07259439]\n",
      "P = \n",
      "[[ 0.12847015  0.0103166  -0.02040403]\n",
      " [ 0.0103166   0.07869789 -0.02907526]\n",
      " [-0.02040403 -0.02907526  0.10023541]]\n",
      "num = \n",
      "344\n",
      "SENSOR 9 4.80175052631 -0.901315690154\n",
      "\n",
      "num = \n",
      "345\n",
      "SENSOR 2 2.53903797695 2.78425180019\n",
      "\n",
      "num = \n",
      "346\n",
      "SENSOR 3 1.21005538358 -0.947107299591\n",
      "\n",
      "num = \n",
      "347\n",
      "SENSOR 7 4.21526740137 -1.77072934034\n",
      "\n",
      "num = \n",
      "348\n",
      "ODOMETRY -0.000470079132191 0.100071592578 0.000364741934559\n",
      "\n",
      "sqP = \n",
      "[[0.3584273251036883, 0.0, 0.0], [0.028782969262330992, 0.27905094732771696, 0.0], [-0.05692655952654023, -0.09832162691775058, 0.2955124892901503]]\n",
      "sigma = \n",
      "[[1.65454432 1.0974784  6.07259439]\n",
      " [2.37139897 1.0974784  6.07259439]\n",
      " [1.71211025 1.65558029 6.07259439]\n",
      " [1.5406912  0.90083514 6.66361937]\n",
      " [0.93768967 1.0974784  6.07259439]\n",
      " [1.59697838 0.5393765  6.07259439]\n",
      " [1.76839744 1.29412165 5.48156941]]\n",
      "sqP = \n",
      "[[0.5766621540209399, 0.0, 0.0], [0.002565409363834176, 0.5347235116973542, 0.0], [-0.030942992729500008, -0.051113038194116646, 0.533064270784415]]\n",
      "m = \n",
      "[1.692589   1.08235788 6.26285522]\n",
      "P = \n",
      "[[ 0.14695179  0.01264569 -0.02614541]\n",
      " [ 0.01264569  0.08033854 -0.03339209]\n",
      " [-0.02614541 -0.03339209  0.10805137]]\n",
      "num = \n",
      "349\n",
      "SENSOR 9 4.8930608011 -0.931458155021\n",
      "\n",
      "num = \n",
      "350\n",
      "SENSOR 2 2.49064672217 2.70820827557\n",
      "\n",
      "num = \n",
      "351\n",
      "SENSOR 3 1.1298112679 -0.927638881263\n",
      "\n",
      "num = \n",
      "352\n",
      "SENSOR 7 4.10863769033 -1.85570810456\n",
      "\n",
      "num = \n",
      "353\n",
      "ODOMETRY 0.000293614872845 0.0999543517986 0.000235684154045\n",
      "\n",
      "sqP = \n",
      "[[0.3833429125274659, 0.0, 0.0], [0.03298792720707198, 0.28151436599980273, 0.0], [-0.06820371510804744, -0.11062382237417398, 0.3019304383709932]]\n",
      "sigma = \n",
      "[[1.692589   1.08235788 6.26285522]\n",
      " [2.45927483 1.08235788 6.26285522]\n",
      " [1.75856486 1.64538661 6.26285522]\n",
      " [1.55618157 0.86111024 6.86671609]\n",
      " [0.92590318 1.08235788 6.26285522]\n",
      " [1.62661315 0.51932915 6.26285522]\n",
      " [1.82899643 1.30360553 5.65899434]]\n",
      "sqP = \n",
      "[[0.593878555498512, 0.0, 0.0], [0.005142299755906393, 0.5379305662929864, 0.0], [-0.03750356866713626, -0.058070003024987236, 0.5357612467054718]]\n",
      "m = \n",
      "[1.73315212 1.10724396 6.26535897]\n",
      "P = \n",
      "[[ 0.10212013  0.01457933 -0.02251854]\n",
      " [ 0.01457933  0.07654882 -0.02949142]\n",
      " [-0.02251854 -0.02949142  0.10935931]]\n",
      "num = \n",
      "354\n",
      "SENSOR 9 4.78120287936 -0.931491845794\n",
      "\n",
      "num = \n",
      "355\n",
      "SENSOR 2 2.65158902922 2.74003752891\n",
      "\n",
      "num = \n",
      "356\n",
      "SENSOR 3 1.13696167911 -1.02091756573\n",
      "\n",
      "num = \n",
      "357\n",
      "SENSOR 7 4.14832981289 -1.93565577604\n",
      "\n",
      "num = \n",
      "358\n",
      "ODOMETRY -8.40851246277e-05 0.0999403556225 -0.000176595962094\n",
      "\n",
      "sqP = \n",
      "[[0.3195624103331346, 0.0, 0.0], [0.04562279043071168, 0.2728871242164997, 0.0], [-0.07046679574905321, -0.09629082157360311, 0.3084182542381289]]\n",
      "sigma = \n",
      "[[1.73315212 1.10724396 6.26535897]\n",
      " [2.37227694 1.10724396 6.26535897]\n",
      " [1.8243977  1.65301821 6.26535897]\n",
      " [1.59221852 0.91466232 6.88219547]\n",
      " [1.09402729 1.10724396 6.26535897]\n",
      " [1.64190653 0.56146972 6.26535897]\n",
      " [1.87408571 1.29982561 5.64852246]]\n",
      "sqP = \n",
      "[[0.556028005057071, 0.0, 0.0], [0.016440256148035997, 0.5297336444389622, 0.0], [-0.042065598978061544, -0.05104138149545224, 0.539945638121468]]\n",
      "m = \n",
      "[1.76948301 1.12324527 6.36398654]\n",
      "P = \n",
      "[[ 0.08819738  0.01554859 -0.02114949]\n",
      " [ 0.01554859  0.07702737 -0.02903339]\n",
      " [-0.02114949 -0.02903339  0.1092104 ]]\n",
      "num = \n",
      "359\n",
      "SENSOR 9 4.57621416752 -0.993635910976\n",
      "\n",
      "num = \n",
      "360\n",
      "SENSOR 2 2.92434868722 2.7082657163\n",
      "\n",
      "num = \n",
      "361\n",
      "SENSOR 3 0.871667628237 -1.08932996938\n",
      "\n",
      "num = \n",
      "362\n",
      "SENSOR 7 4.2336588264 -1.9442154823\n",
      "\n",
      "num = \n",
      "363\n",
      "ODOMETRY -0.00962491243249 0.100226987653 -0.0205912870107\n",
      "\n",
      "sqP = \n",
      "[[0.2969804442853354, 0.0, 0.0], [0.052355613630275355, 0.27255505581747813, 0.0], [-0.07121508943327673, -0.09284318999607796, 0.30906140817574607]]\n",
      "sigma = \n",
      "[[1.76948301 1.12324527 6.36398654]\n",
      " [2.3634439  1.12324527 6.36398654]\n",
      " [1.87419424 1.66835538 6.36398654]\n",
      " [1.62705284 0.93755889 6.98210935]\n",
      " [1.17552213 1.12324527 6.36398654]\n",
      " [1.66477179 0.57813516 6.36398654]\n",
      " [1.91191319 1.30893165 5.74586372]]\n",
      "sqP = \n",
      "[[0.5440680872184175, 0.0, 0.0], [0.022012544835614583, 0.5283630205903596, 0.0], [-0.043748343537150805, -0.04889936294494509, 0.5405202782213382]]\n",
      "m = \n",
      "[1.80204149 1.18921655 6.59868502]\n",
      "P = \n",
      "[[ 0.08400834  0.01554623 -0.02047615]\n",
      " [ 0.01554623  0.07554665 -0.02647971]\n",
      " [-0.02047615 -0.02647971  0.10874268]]\n",
      "num = \n",
      "364\n",
      "SENSOR 9 4.44837110071 -0.989307488326\n",
      "\n",
      "num = \n",
      "365\n",
      "SENSOR 2 2.8892743732 2.84845502425\n",
      "\n",
      "num = \n",
      "366\n",
      "SENSOR 3 0.891192383698 -1.23984271\n",
      "\n",
      "num = \n",
      "367\n",
      "SENSOR 7 4.24686986677 -1.92323203717\n",
      "\n",
      "num = \n",
      "368\n",
      "ODOMETRY -0.0102033452948 0.0998839630755 -0.0215571904424\n",
      "\n",
      "sqP = \n",
      "[[0.28984191940108606, 0.0, 0.0], [0.05363692325671278, 0.2695732331118868, 0.0], [-0.0706459275621531, -0.08417186249626149, 0.31091306251943]]\n",
      "sigma = \n",
      "[[1.80204149 1.18921655 6.59868502]\n",
      " [2.38172533 1.18921655 6.59868502]\n",
      " [1.90931534 1.72836302 6.59868502]\n",
      " [1.66074964 1.02087283 7.22051115]\n",
      " [1.22235765 1.18921655 6.59868502]\n",
      " [1.69476765 0.65007009 6.59868502]\n",
      " [1.94333335 1.35756028 5.97685889]]\n",
      "sqP = \n",
      "[[0.540255592159553, 0.0, 0.0], [0.021983576024102, 0.5254374343554348, 0.0], [-0.044213232226329814, -0.04441611141137087, 0.5420231238642128]]\n",
      "m = \n",
      "[1.85394708 1.21246835 6.67944669]\n",
      "P = \n",
      "[[ 0.08289858  0.0145717  -0.01921617]\n",
      " [ 0.0145717   0.07180018 -0.01911653]\n",
      " [-0.01921617 -0.01911653  0.10680374]]\n",
      "num = \n",
      "369\n",
      "SENSOR 9 4.51127976731 -0.965675234448\n",
      "\n",
      "num = \n",
      "370\n",
      "SENSOR 2 2.99371450745 2.84262811823\n",
      "\n",
      "num = \n",
      "371\n",
      "SENSOR 3 0.799137718613 -1.19111999767\n",
      "\n",
      "num = \n",
      "372\n",
      "SENSOR 7 4.34718168559 -1.87200596541\n",
      "\n",
      "num = \n",
      "373\n",
      "ODOMETRY -0.00860842707999 0.0998102570683 -0.0193391675193\n",
      "\n",
      "sqP = \n",
      "[[0.287921131858923, 0.0, 0.0], [0.05061003931048465, 0.26313267089736875, 0.0], [-0.06674109196152098, -0.05981302186565561, 0.3142797647480605]]\n",
      "sigma = \n",
      "[[1.85394708 1.21246835 6.67944669]\n",
      " [2.42978934 1.21246835 6.67944669]\n",
      " [1.95516716 1.73873369 6.67944669]\n",
      " [1.72046489 1.09284231 7.30800622]\n",
      " [1.27810481 1.21246835 6.67944669]\n",
      " [1.752727   0.68620301 6.67944669]\n",
      " [1.98742926 1.33209439 6.05088716]]\n",
      "sqP = \n",
      "[[0.5384369300170153, 0.0, 0.0], [0.01689284039796021, 0.5195783393562589, 0.0], [-0.04322620877052597, -0.03141633098808039, 0.5451090926903884]]\n",
      "m = \n",
      "[1.9062856  1.1926563  6.75693954]\n",
      "P = \n",
      "[[ 0.08226691  0.01372462 -0.01813458]\n",
      " [ 0.01372462  0.0704532  -0.0153327 ]\n",
      " [-0.01813458 -0.0153327   0.1048379 ]]\n",
      "num = \n",
      "374\n",
      "SENSOR 9 4.45640142154 -0.978412228293\n",
      "\n",
      "num = \n",
      "375\n",
      "SENSOR 2 3.26617603062 2.8384001977\n",
      "\n",
      "num = \n",
      "376\n",
      "SENSOR 3 0.816222604143 -1.30597066763\n",
      "\n",
      "num = \n",
      "377\n",
      "SENSOR 7 4.2980455787 -1.89648658794\n",
      "\n",
      "num = \n",
      "378\n",
      "ODOMETRY -0.00956022288558 0.100150972292 -0.0200463224548\n",
      "\n",
      "sqP = \n",
      "[[0.28682208032395773, 0.0, 0.0], [0.047850633421476926, 0.26108143153777325, 0.0], [-0.06322589996270307, -0.04713970182002922, 0.31403540024887655]]\n",
      "sigma = \n",
      "[[1.9062856  1.1926563  6.75693954]\n",
      " [2.47992976 1.1926563  6.75693954]\n",
      " [2.00198687 1.71481916 6.75693954]\n",
      " [1.7798338  1.09837689 7.38501034]\n",
      " [1.33264144 1.1926563  6.75693954]\n",
      " [1.81058434 0.67049344 6.75693954]\n",
      " [2.0327374  1.2869357  6.12886874]]\n",
      "sqP = \n",
      "[[0.5371723217984087, 0.0, 0.0], [0.014005996202094506, 0.5176754315679201, 0.0], [-0.04197996331768859, -0.024360033717645103, 0.5455773013546963]]\n",
      "m = \n",
      "[1.97312427 1.22365922 6.8994417 ]\n",
      "P = \n",
      "[[ 0.08173039  0.01277144 -0.01702401]\n",
      " [ 0.01277144  0.06841511 -0.0121693 ]\n",
      " [-0.01702401 -0.0121693   0.10346619]]\n",
      "num = \n",
      "379\n",
      "SENSOR 9 4.37573768277 -0.917844889679\n",
      "\n",
      "num = \n",
      "380\n",
      "SENSOR 2 3.317865598 2.95007305006\n",
      "\n",
      "num = \n",
      "381\n",
      "SENSOR 3 0.817247142217 -1.37869382939\n",
      "\n",
      "num = \n",
      "382\n",
      "SENSOR 7 4.09119325406 -1.94192716645\n",
      "\n",
      "num = \n",
      "383\n",
      "ODOMETRY -0.0103002834124 0.100007766723 -0.0197368000329\n",
      "\n",
      "sqP = \n",
      "[[0.2858852688990509, 0.0, 0.0], [0.04467329751954609, 0.2577196195108462, 0.0], [-0.05954840960332321, -0.036896991686293734, 0.3139407487249471]]\n",
      "sigma = \n",
      "[[1.97312427 1.22365922 6.8994417 ]\n",
      " [2.5448948  1.22365922 6.8994417 ]\n",
      " [2.06247086 1.73909846 6.8994417 ]\n",
      " [1.85402745 1.14986524 7.5273232 ]\n",
      " [1.40135373 1.22365922 6.8994417 ]\n",
      " [1.88377767 0.70821999 6.8994417 ]\n",
      " [2.09222109 1.29745321 6.2715602 ]]\n",
      "sqP = \n",
      "[[0.5359777080915191, 0.0, 0.0], [0.011272634566211092, 0.5155589316170742, 0.0], [-0.04080705111311107, -0.018735907806581586, 0.5460099427294524]]\n",
      "m = \n",
      "[2.08855169 1.30882827 6.92880438]\n",
      "P = \n",
      "[[ 0.08169823  0.01197018 -0.01537072]\n",
      " [ 0.01197018  0.06590734 -0.00700207]\n",
      " [-0.01537072 -0.00700207  0.10168772]]\n",
      "num = \n",
      "384\n",
      "SENSOR 9 4.21811509958 -0.851878267922\n",
      "\n",
      "num = \n",
      "385\n",
      "SENSOR 2 3.18846120949 2.97202181201\n",
      "\n",
      "num = \n",
      "386\n",
      "SENSOR 3 1.13340957174 -1.46851281333\n",
      "\n",
      "num = \n",
      "387\n",
      "SENSOR 7 4.38585468217 -1.82603901749\n",
      "\n",
      "num = \n",
      "388\n",
      "ODOMETRY -0.0102831048513 0.10010953823 -0.0202530209795\n",
      "\n",
      "sqP = \n",
      "[[0.28582901736751154, 0.0, 0.0], [0.0418788225581855, 0.2532854173788177, 0.0], [-0.05377593605792388, -0.0187535401401556, 0.3137581423147132]]\n",
      "sigma = \n",
      "[[2.08855169 1.30882827 6.92880438]\n",
      " [2.66020972 1.30882827 6.92880438]\n",
      " [2.17230933 1.81539911 6.92880438]\n",
      " [1.98099981 1.27132119 7.55632066]\n",
      " [1.51689365 1.30882827 6.92880438]\n",
      " [2.00479404 0.80225744 6.92880438]\n",
      " [2.19610356 1.34633535 6.30128809]]\n",
      "sqP = \n",
      "[[0.5351111232697776, 0.0, 0.0], [0.00832008113460429, 0.5128480503197194, 0.0], [-0.03897464859437335, -0.008463659508175356, 0.5465427440971314]]\n",
      "m = \n",
      "[2.20189966 1.18645175 7.00997179]\n",
      "P = \n",
      "[[ 0.06896487 -0.00124872 -0.00188295]\n",
      " [-0.00124872  0.06858417 -0.00812217]\n",
      " [-0.00188295 -0.00812217  0.10306831]]\n",
      "num = \n",
      "389\n",
      "SENSOR 9 4.26699097617 -0.944178359109\n",
      "\n",
      "num = \n",
      "390\n",
      "SENSOR 2 3.38023967491 3.01452503314\n",
      "\n",
      "num = \n",
      "391\n",
      "SENSOR 3 0.720668136763 -1.55711368901\n",
      "\n",
      "num = \n",
      "392\n",
      "SENSOR 7 4.33394842239 -1.91875382264\n",
      "\n",
      "num = \n",
      "393\n",
      "ODOMETRY -0.0105866501206 0.0999017879145 -0.0197382893367\n",
      "\n",
      "sqP = \n",
      "[[0.26261163124121967, 0.0, 0.0], [-0.004755001228376122, 0.2618426207064119, 0.0], [-0.007170102912776129, -0.031149482138137466, 0.31944734479916537]]\n",
      "sigma = \n",
      "[[2.20189966 1.18645175 7.00997179]\n",
      " [2.72712292 1.18645175 7.00997179]\n",
      " [2.19238966 1.71013699 7.00997179]\n",
      " [2.18755945 1.12415278 7.64886648]\n",
      " [1.6766764  1.18645175 7.00997179]\n",
      " [2.21140966 0.66276651 7.00997179]\n",
      " [2.21623986 1.24875071 6.3710771 ]]\n",
      "sqP = \n",
      "[[0.5186895885552707, 0.0, 0.0], [-0.012317284062112003, 0.5195914887013361, 0.0], [-0.01181585822612684, -0.018414283605526643, 0.549609523503571]]\n",
      "m = \n",
      "[2.3116873  1.23644603 7.09212936]\n",
      "P = \n",
      "[[ 0.06589775 -0.00515284  0.00369275]\n",
      " [-0.00515284  0.06488869 -0.00450046]\n",
      " [ 0.00369275 -0.00450046  0.10164124]]\n",
      "num = \n",
      "394\n",
      "SENSOR 9 4.22270883023 -0.860892437552\n",
      "\n",
      "num = \n",
      "395\n",
      "SENSOR 2 3.36042267996 3.01995506444\n",
      "\n",
      "num = \n",
      "396\n",
      "SENSOR 3 0.717148685032 -1.66529431909\n",
      "\n",
      "num = \n",
      "397\n",
      "SENSOR 7 4.43948581701 -1.84649833616\n",
      "\n",
      "num = \n",
      "398\n",
      "ODOMETRY -0.0113245534652 0.100123041114 -0.0190696769167\n",
      "\n",
      "sqP = \n",
      "[[0.2567055802560996, 0.0, 0.0], [-0.02007296556974558, 0.2539404767164681, 0.0], [0.014385167502956089, -0.01658539997422546, 0.3180553864697177]]\n",
      "sigma = \n",
      "[[2.3116873  1.23644603 7.09212936]\n",
      " [2.82509846 1.23644603 7.09212936]\n",
      " [2.27154137 1.74432699 7.09212936]\n",
      " [2.34045763 1.20327523 7.72824013]\n",
      " [1.79827614 1.23644603 7.09212936]\n",
      " [2.35183323 0.72856508 7.09212936]\n",
      " [2.28291696 1.26961683 6.45601859]]\n",
      "sqP = \n",
      "[[0.5162437524323883, 0.0, 0.0], [-0.01951217182548121, 0.5152914895598584, 0.0], [0.0006457342394008157, -0.009626867379464532, 0.548401405940963]]\n",
      "m = \n",
      "[1.97207021 1.18700138 7.08331369]\n",
      "P = \n",
      "[[ 0.12734911 -0.00923897 -0.01130348]\n",
      " [-0.00923897  0.06110758 -0.01742782]\n",
      " [-0.01130348 -0.01742782  0.09457899]]\n",
      "num = \n",
      "399\n",
      "SENSOR 9 4.16843098657 -0.834433300637\n",
      "\n",
      "num = \n",
      "400\n",
      "SENSOR 2 3.51256815216 2.97921133252\n",
      "\n",
      "num = \n",
      "401\n",
      "SENSOR 3 0.834830514938 -1.73903910734\n",
      "\n",
      "num = \n",
      "402\n",
      "SENSOR 7 4.28359501376 -1.86953074931\n",
      "\n",
      "num = \n",
      "403\n",
      "ODOMETRY -0.00971082754926 0.100025950554 -0.0198211377492\n",
      "\n",
      "sqP = \n",
      "[[0.35686007595561137, 0.0, 0.0], [-0.025889618405217574, 0.24584000467545833, 0.0], [-0.03167482656163733, -0.07422659935762858, 0.296759336923079]]\n",
      "sigma = \n",
      "[[1.97207021 1.18700138 7.08331369]\n",
      " [2.68579036 1.18700138 7.08331369]\n",
      " [1.92029097 1.67868139 7.08331369]\n",
      " [1.90872056 1.03854819 7.67683236]\n",
      " [1.25835006 1.18700138 7.08331369]\n",
      " [2.02384945 0.69532137 7.08331369]\n",
      " [2.03541986 1.33545458 6.48979501]]\n",
      "sqP = \n",
      "[[0.5736049867198875, 0.0, 0.0], [-0.026125644612747145, 0.516678524218532, 0.0], [-0.02424897293649128, -0.041212897433403506, 0.5354878051138133]]\n",
      "m = \n",
      "[1.9987352  1.25094457 7.19923686]\n",
      "P = \n",
      "[[ 0.08540402 -0.01049067 -0.00127959]\n",
      " [-0.01049067  0.06297792 -0.00290579]\n",
      " [-0.00127959 -0.00290579  0.1014769 ]]\n",
      "num = \n",
      "404\n",
      "SENSOR 9 4.02856295399 -0.968273518321\n",
      "\n",
      "num = \n",
      "405\n",
      "SENSOR 2 3.63658640677 3.12915861055\n",
      "\n",
      "num = \n",
      "406\n",
      "SENSOR 3 0.734021188267 -1.75470305372\n",
      "\n",
      "num = \n",
      "407\n",
      "SENSOR 7 4.54650380966 -1.76609368488\n",
      "\n",
      "num = \n",
      "408\n",
      "ODOMETRY -0.010589475487 0.10010496938 -0.0199922914546\n",
      "\n",
      "sqP = \n",
      "[[0.2922396544836051, 0.0, 0.0], [-0.03589748528681041, 0.24837329819395895, 0.0], [-0.004378569077977069, -0.012332109945704063, 0.31828548654186606]]\n",
      "sigma = \n",
      "[[1.9987352  1.25094457 7.19923686]\n",
      " [2.58321451 1.25094457 7.19923686]\n",
      " [1.92694023 1.74769117 7.19923686]\n",
      " [1.98997806 1.22628035 7.83580783]\n",
      " [1.41425589 1.25094457 7.19923686]\n",
      " [2.07053017 0.75419798 7.19923686]\n",
      " [2.00749234 1.27560879 6.56266588]]\n",
      "sqP = \n",
      "[[0.535454775839925, 0.0, 0.0], [-0.030536317059566474, 0.5130134564406948, 0.0], [-0.008818747241797098, -0.006703242875243261, 0.549059804227818]]\n",
      "m = \n",
      "[2.09967033 1.26581249 7.37118743]\n",
      "P = \n",
      "[[ 0.07266112 -0.00990848  0.00385595]\n",
      " [-0.00990848  0.06590046  0.00087119]\n",
      " [ 0.00385595  0.00087119  0.09797815]]\n",
      "num = \n",
      "409\n",
      "SENSOR 9 3.91015885795 -0.869086963006\n",
      "\n",
      "num = \n",
      "410\n",
      "SENSOR 2 3.87255632648 3.16604194771\n",
      "\n",
      "num = \n",
      "411\n",
      "SENSOR 3 0.865196059921 -1.86949836389\n",
      "\n",
      "num = \n",
      "412\n",
      "SENSOR 7 4.51133867781 -1.81430214087\n",
      "\n",
      "num = \n",
      "413\n",
      "ODOMETRY -0.00946128189441 0.0997349532468 -0.0199718063448\n",
      "\n",
      "sqP = \n",
      "[[0.2695572721425511, 0.0, 0.0], [-0.03675835048810717, 0.2540655151903196, 0.0], [0.014304767185780621, 0.005498601189429311, 0.312639225271339]]\n",
      "sigma = \n",
      "[[2.09967033 1.26581249 7.37118743]\n",
      " [2.63878488 1.26581249 7.37118743]\n",
      " [2.02615363 1.77394352 7.37118743]\n",
      " [2.12827987 1.27680969 7.99646588]\n",
      " [1.56055579 1.26581249 7.37118743]\n",
      " [2.17318703 0.75768146 7.37118743]\n",
      " [2.0710608  1.25481528 6.74590898]]\n",
      "sqP = \n",
      "[[0.523657259722215, 0.0, 0.0], [-0.029432192491214433, 0.5155981575010309, 0.0], [0.0018405661728264876, 0.004107891898943433, 0.5454113054952746]]\n",
      "m = \n",
      "[2.1883638  1.25218311 7.41094839]\n",
      "P = \n",
      "[[ 0.07168045 -0.01018154 -0.00674841]\n",
      " [-0.01018154  0.06114191 -0.01102828]\n",
      " [-0.00674841 -0.01102828  0.09045688]]\n",
      "num = \n",
      "414\n",
      "SENSOR 9 3.82613593432 -0.874876680077\n",
      "\n",
      "num = \n",
      "415\n",
      "SENSOR 2 3.83879660579 -3.13229830775\n",
      "\n",
      "num = \n",
      "416\n",
      "SENSOR 3 1.06514315073 -1.87293500873\n",
      "\n",
      "num = \n",
      "417\n",
      "SENSOR 7 4.56299343999 -1.75934253741\n",
      "\n",
      "num = \n",
      "418\n",
      "ODOMETRY -0.00979951516687 0.0997189457445 -0.0206080614093\n",
      "\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "sqP = \n",
      "[[0.2677320546449223, 0.0, 0.0], [-0.03802883927552095, 0.24432707196610373, 0.0], [-0.025205827372819973, -0.04906056746434029, 0.29565961506330934]]\n",
      "sigma = \n",
      "[[2.1883638  1.25218311 7.41094839]\n",
      " [2.72382791 1.25218311 7.41094839]\n",
      " [2.11230613 1.74083725 7.41094839]\n",
      " [2.13795215 1.15406197 8.00226762]\n",
      " [1.6528997  1.25218311 7.41094839]\n",
      " [2.26442148 0.76352896 7.41094839]\n",
      " [2.23877546 1.35030424 6.81962916]]\n",
      "sqP = \n",
      "[[0.5232226862766785, 0.0, 0.0], [-0.026405895226039252, 0.5130150061180803, 0.0], [-0.021908086428231936, -0.026741953560675453, 0.5360136374324276]]\n",
      "m = \n",
      "[1.34506257 1.23104985 7.54433425]\n",
      "P = \n",
      "[[ 0.07241402 -0.00935471 -0.01203147]\n",
      " [-0.00935471  0.0600726  -0.010533  ]\n",
      " [-0.01203147 -0.010533    0.0887617 ]]\n",
      "num = \n",
      "419\n",
      "SENSOR 9 4.12881342913 -0.867651605124\n",
      "\n",
      "num = \n",
      "420\n",
      "SENSOR 2 4.0288094419 -3.2276561161\n",
      "\n",
      "num = \n",
      "421\n",
      "SENSOR 3 0.901200006923 -2.03251122022\n",
      "\n",
      "num = \n",
      "422\n",
      "SENSOR 7 4.62001856502 -1.8309460386\n",
      "\n",
      "num = \n",
      "423\n",
      "ODOMETRY -0.00943323576827 0.100124414548 -0.0203296697597\n",
      "\n",
      "sqP = \n",
      "[[0.26909852810682083, 0.0, 0.0], [-0.03476314470790119, 0.24261929486213227, 0.0], [-0.04471027680271788, -0.049819919175370354, 0.29031132480964716]]\n",
      "sigma = \n",
      "[[1.34506257 1.23104985 7.54433425]\n",
      " [1.88325963 1.23104985 7.54433425]\n",
      " [1.27553628 1.71628844 7.54433425]\n",
      " [1.25564202 1.13141001 8.1249569 ]\n",
      " [0.80686551 1.23104985 7.54433425]\n",
      " [1.41458886 0.74581126 7.54433425]\n",
      " [1.43448312 1.33068969 6.9637116 ]]\n",
      "sqP = \n",
      "[[0.524996669428498, 0.0, 0.0], [-0.025152510859111778, 0.5124120492633075, 0.0], [-0.02152472596690278, -0.027398533430534375, 0.5315400962302145]]\n",
      "m = \n",
      "[0.99561756 1.04654132 7.86974044]\n",
      "P = \n",
      "[[ 0.13819616 -0.00821009  0.00302057]\n",
      " [-0.00821009  0.05529454  0.01798819]\n",
      " [ 0.00302057  0.01798819  0.07820174]]\n",
      "num = \n",
      "424\n",
      "SENSOR 9 3.70232136893 -0.94416511906\n",
      "\n",
      "num = \n",
      "425\n",
      "SENSOR 2 3.98368634906 -3.07682994555\n",
      "\n",
      "num = \n",
      "426\n",
      "SENSOR 3 0.899772694406 -2.09221030348\n",
      "\n",
      "num = \n",
      "427\n",
      "SENSOR 7 4.46139118814 -1.86311006595\n",
      "\n",
      "num = \n",
      "428\n",
      "ODOMETRY -0.0100890424076 0.0999779958111 -0.0198161117798\n",
      "\n",
      "sqP = \n",
      "[[0.37174743663504456, 0.0, 0.0], [-0.022085140768799097, 0.23410849857083543, 0.0], [0.008125316180814038, 0.07760351104531868, 0.268539411706587]]\n",
      "sigma = \n",
      "[[0.99561756 1.04654132 7.86974044]\n",
      " [1.73911244 1.04654132 7.86974044]\n",
      " [0.95144728 1.51475832 7.86974044]\n",
      " [1.0118682  1.20174834 8.40681926]\n",
      " [0.25212269 1.04654132 7.86974044]\n",
      " [1.03978785 0.57832432 7.86974044]\n",
      " [0.97936693 0.8913343  7.33266161]]\n",
      "sqP = \n",
      "[[0.582022276936667, 0.0, 0.0], [-0.025928187782613348, 0.5116217131620922, 0.0], [0.015745825379666324, 0.039763772791465354, 0.5205455094087735]]\n",
      "m = \n",
      "[0.96336677 1.45269398 7.75413563]\n",
      "P = \n",
      "[[ 0.14532415 -0.00834274 -0.01215675]\n",
      " [-0.00834274  0.07372036  0.00862799]\n",
      " [-0.01215675  0.00862799  0.08517652]]\n",
      "num = \n",
      "429\n",
      "SENSOR 9 3.7518939771 -0.918292146011\n",
      "\n",
      "num = \n",
      "430\n",
      "SENSOR 2 4.21633761598 -3.02832665334\n",
      "\n",
      "num = \n",
      "431\n",
      "SENSOR 3 1.18763314519 -2.0979866523\n",
      "\n",
      "num = \n",
      "432\n",
      "SENSOR 7 4.57848442895 -1.89875432575\n",
      "\n",
      "num = \n",
      "433\n",
      "ODOMETRY -0.00985362326451 0.100067538531 -0.0193174898999\n",
      "\n",
      "sqP = \n",
      "[[0.3812140519445587, 0.0, 0.0], [-0.021884658300558853, 0.2706315149557821, 0.0], [-0.03188955373097022, 0.02930219543871107, 0.28861904945768546]]\n",
      "sigma = \n",
      "[[0.96336677 1.45269398 7.75413563]\n",
      " [1.72579487 1.45269398 7.75413563]\n",
      " [0.91959745 1.99395701 7.75413563]\n",
      " [0.89958766 1.51129837 8.33137373]\n",
      " [0.20093867 1.45269398 7.75413563]\n",
      " [1.00713609 0.91143095 7.75413563]\n",
      " [1.02714588 1.39408959 7.17689753]]\n",
      "sqP = \n",
      "[[0.5889142851944427, 0.0, 0.0], [-0.030072525206223864, 0.5245601256413495, 0.0], [-0.0025410202799486456, 0.015691647735979128, 0.5314911499017198]]\n",
      "m = \n",
      "[0.91357363 1.50978308 7.87994836]\n",
      "P = \n",
      "[[ 0.16310866  0.00195572 -0.00782418]\n",
      " [ 0.00195572  0.08177683  0.00074232]\n",
      " [-0.00782418  0.00074232  0.0824152 ]]\n",
      "num = \n",
      "434\n",
      "SENSOR 9 3.59551880688 -0.808067235008\n",
      "\n",
      "num = \n",
      "435\n",
      "SENSOR 2 4.17678928397 -2.9234396319\n",
      "\n",
      "num = \n",
      "436\n",
      "SENSOR 3 1.07794432908 -2.21510076284\n",
      "\n",
      "num = \n",
      "437\n",
      "SENSOR 7 4.51763234841 -1.82762500095\n",
      "\n",
      "num = \n",
      "438\n",
      "ODOMETRY -0.0106424165584 0.0998962699375 -0.0193739099135\n",
      "\n",
      "sqP = \n",
      "[[0.40386713179753225, 0.0, 0.0], [0.004842478620769578, 0.28592547890226294, 0.0], [-0.01937315777275462, 0.0029243020863050596, 0.2864111247398909]]\n",
      "sigma = \n",
      "[[0.91357363 1.50978308 7.87994836]\n",
      " [1.7213079  1.50978308 7.87994836]\n",
      " [0.92325859 2.08163404 7.87994836]\n",
      " [0.87482732 1.51563169 8.45277061]\n",
      " [0.10583937 1.50978308 7.87994836]\n",
      " [0.90388868 0.93793212 7.87994836]\n",
      " [0.95231995 1.50393448 7.30712611]]\n",
      "sqP = \n",
      "[[0.6029157727142705, 0.0, 0.0], [-0.016879394415615298, 0.5311743568183355, 0.0], [0.005824040926053499, 0.001701095797512978, 0.5309614363426332]]\n",
      "m = \n",
      "[0.84058079 1.61802854 7.88748208]\n",
      "P = \n",
      "[[ 0.16981689  0.00366012 -0.0065501 ]\n",
      " [ 0.00366012  0.08333134  0.00100983]\n",
      " [-0.0065501   0.00100983  0.07821485]]\n",
      "num = \n",
      "439\n",
      "SENSOR 9 3.54948085637 -0.797013500387\n",
      "\n",
      "num = \n",
      "440\n",
      "SENSOR 2 4.28131949654 -2.97537943488\n",
      "\n",
      "num = \n",
      "441\n",
      "SENSOR 3 1.24246440412 -2.3096004705\n",
      "\n",
      "num = \n",
      "442\n",
      "SENSOR 7 4.5411888941 -1.80078211601\n",
      "\n",
      "num = \n",
      "443\n",
      "ODOMETRY -0.0104250459457 0.100075632863 -0.0188431839508\n",
      "\n",
      "sqP = \n",
      "[[0.4120884500749923, 0.0, 0.0], [0.008881884884015763, 0.28853501850953617, 0.0], [-0.015894879218789657, 0.003989132068186143, 0.27918863623096646]]\n",
      "sigma = \n",
      "[[0.84058079 1.61802854 7.88748208]\n",
      " [1.66475769 1.61802854 7.88748208]\n",
      " [0.85834456 2.19509858 7.88748208]\n",
      " [0.80879104 1.6260068  8.44585935]\n",
      " [0.01640389 1.61802854 7.88748208]\n",
      " [0.82281702 1.0409585  7.88748208]\n",
      " [0.87237055 1.61005028 7.3291048 ]]\n",
      "sqP = \n",
      "[[0.608398246011346, 0.0, 0.0], [-0.014255379678491359, 0.5324640268143763, 0.0], [0.009502343694929725, 0.0021964238995555363, 0.527229892038664]]\n",
      "m = \n",
      "[0.77545496 1.66458676 7.98241031]\n",
      "P = \n",
      "[[ 0.17183542  0.00408089 -0.00574584]\n",
      " [ 0.00408089  0.08470774 -0.00106761]\n",
      " [-0.00574584 -0.00106761  0.07618924]]\n",
      "num = \n",
      "444\n",
      "SENSOR 9 3.48168308597 -0.705073049043\n",
      "\n",
      "num = \n",
      "445\n",
      "SENSOR 2 4.51016536991 -2.85015444599\n",
      "\n",
      "num = \n",
      "446\n",
      "SENSOR 3 1.17397867689 -2.26384820849\n",
      "\n",
      "num = \n",
      "447\n",
      "SENSOR 7 4.58368921745 -1.81442387473\n",
      "\n",
      "num = \n",
      "448\n",
      "ODOMETRY -0.0101771473921 0.100011050735 -0.0196083731561\n",
      "\n",
      "sqP = \n",
      "[[0.41453036489283845, 0.0, 0.0], [0.009844603303016538, 0.29087938954827847, 0.0], [-0.013861072083527541, -0.003201162939899468, 0.27565715165271504]]\n",
      "sigma = \n",
      "[[ 0.77545496  1.66458676  7.98241031]\n",
      " [ 1.60451569  1.66458676  7.98241031]\n",
      " [ 0.79514417  2.24634554  7.98241031]\n",
      " [ 0.74773281  1.65818443  8.53372461]\n",
      " [-0.05360577  1.66458676  7.98241031]\n",
      " [ 0.75576575  1.08282798  7.98241031]\n",
      " [ 0.8031771   1.67098909  7.431096  ]]\n",
      "sqP = \n",
      "[[0.6100200561880637, 0.0, 0.0], [-0.012628235710696205, 0.5336810551798743, 0.0], [0.011855118644888822, -0.001575481853500358, 0.5254490364417032]]\n",
      "m = \n",
      "[0.71575756 1.73659959 8.08495796]\n",
      "P = \n",
      "[[ 0.17272524  0.00395413 -0.00547264]\n",
      " [ 0.00395413  0.08560268 -0.00073241]\n",
      " [-0.00547264 -0.00073241  0.07334088]]\n",
      "num = \n",
      "449\n",
      "SENSOR 9 3.53348326087 -0.784447831034\n",
      "\n",
      "num = \n",
      "450\n",
      "SENSOR 2 4.52117731575 -2.89659106551\n",
      "\n",
      "num = \n",
      "451\n",
      "SENSOR 3 1.39890586374 -2.14286560722\n",
      "\n",
      "num = \n",
      "452\n",
      "SENSOR 7 4.76172397197 -1.82219242799\n",
      "\n",
      "num = \n",
      "453\n",
      "ODOMETRY -0.0101785548489 0.100004838788 -0.0202689362584\n",
      "\n",
      "sqP = \n",
      "[[0.4156022631167126, 0.0, 0.0], [0.009514219387114007, 0.2924246165998156, 0.0], [-0.013167963724213763, -0.00207619562111002, 0.27048691646597833]]\n",
      "sigma = \n",
      "[[ 0.71575756  1.73659959  8.08495796]\n",
      " [ 1.54696208  1.73659959  8.08495796]\n",
      " [ 0.734786    2.32144882  8.08495796]\n",
      " [ 0.68942163  1.7324472   8.62593179]\n",
      " [-0.11544697  1.73659959  8.08495796]\n",
      " [ 0.69672912  1.15175036  8.08495796]\n",
      " [ 0.74209348  1.74075198  7.54398413]]\n",
      "sqP = \n",
      "[[0.6107283821363811, 0.0, 0.0], [-0.01172951981776774, 0.5345136512122792, 0.0], [0.01333731843708971, -0.0010045693490918115, 0.5227983023124969]]\n",
      "m = \n",
      "[0.69105156 1.66637819 8.21488019]\n",
      "P = \n",
      "[[ 0.17351826  0.00363612 -0.00569644]\n",
      " [ 0.00363612  0.08661105 -0.0006446 ]\n",
      " [-0.00569644 -0.0006446   0.07070578]]\n",
      "num = \n",
      "454\n",
      "SENSOR 9 3.44733618805 -0.725368623109\n",
      "\n",
      "num = \n",
      "455\n",
      "SENSOR 2 4.69146013992 -2.87115134275\n",
      "\n",
      "num = \n",
      "456\n",
      "SENSOR 3 1.50765435383 -2.22245197353\n",
      "\n",
      "num = \n",
      "457\n",
      "SENSOR 7 4.48642435312 -1.84876318884\n",
      "\n",
      "num = \n",
      "458\n",
      "ODOMETRY -0.0091227397028 0.0999924972858 -0.0181685651638\n",
      "\n",
      "sqP = \n",
      "[[0.4165552294384041, 0.0, 0.0], [0.00872902055926767, 0.29416807302680753, 0.0], [-0.013675106048398585, -0.0017854746700533618, 0.26554771098935703]]\n",
      "sigma = \n",
      "[[ 0.69105156  1.66637819  8.21488019]\n",
      " [ 1.52416201  1.66637819  8.21488019]\n",
      " [ 0.7085096   2.25471433  8.21488019]\n",
      " [ 0.66370134  1.66280724  8.74597561]\n",
      " [-0.1420589   1.66637819  8.21488019]\n",
      " [ 0.67359351  1.07804204  8.21488019]\n",
      " [ 0.71840177  1.66994914  7.68378477]]\n",
      "sqP = \n",
      "[[0.6113766952524774, 0.0, 0.0], [-0.0116571877449027, 0.5354816839441984, 0.0], [0.01368384122986655, -0.000854186684686984, 0.5202606761467766]]\n",
      "m = \n",
      "[0.66240258 1.78728654 8.27490162]\n",
      "P = \n",
      "[[ 0.17424232  0.00299658 -0.00563613]\n",
      " [ 0.00299658  0.08672419  0.00209581]\n",
      " [-0.00563613  0.00209581  0.06997003]]\n",
      "num = \n",
      "459\n",
      "SENSOR 9 3.22534891044 -0.724699872244\n",
      "\n",
      "num = \n",
      "460\n",
      "SENSOR 2 4.82403976411 -2.9345124872\n",
      "\n",
      "num = \n",
      "461\n",
      "SENSOR 3 1.40944143312 -2.41378091735\n",
      "\n",
      "num = \n",
      "462\n",
      "SENSOR 7 4.61353587116 -1.78898785581\n",
      "\n",
      "num = \n",
      "463\n",
      "ODOMETRY -0.00990767375964 0.0999963798923 -0.0204463659192\n",
      "\n",
      "sqP = \n",
      "[[0.4174234264095788, 0.0, 0.0], [0.00717874379841238, 0.294402192920519, 0.0], [-0.01350218920885841, 0.007448106108228319, 0.26406864477946285]]\n",
      "sigma = \n",
      "[[ 0.66240258  1.78728654  8.27490162]\n",
      " [ 1.49724943  1.78728654  8.27490162]\n",
      " [ 0.67676006  2.37609093  8.27490162]\n",
      " [ 0.6353982   1.80218276  8.80303891]\n",
      " [-0.17244428  1.78728654  8.27490162]\n",
      " [ 0.64804509  1.19848216  8.27490162]\n",
      " [ 0.68940695  1.77239033  7.74676433]]\n",
      "sqP = \n",
      "[[0.6119445729732949, 0.0, 0.0], [-0.01209742549027267, 0.5356984981055827, 0.0], [0.014276538875252012, 0.003639697128384614, 0.5195105257535078]]\n",
      "m = \n",
      "[0.6131384  1.91179178 8.34961331]\n",
      "P = \n",
      "[[ 0.17478762  0.00262147 -0.00601547]\n",
      " [ 0.00262147  0.08779768  0.00073878]\n",
      " [-0.00601547  0.00073878  0.06849919]]\n",
      "num = \n",
      "464\n",
      "SENSOR 9 3.2978264557 -0.848937354757\n",
      "\n",
      "num = \n",
      "465\n",
      "SENSOR 2 4.79210487087 -2.88027569823\n",
      "\n",
      "num = \n",
      "466\n",
      "SENSOR 3 1.57460267827 -2.35009049783\n",
      "\n",
      "num = \n",
      "467\n",
      "SENSOR 7 4.86239210585 -1.68728512464\n",
      "\n",
      "num = \n",
      "468\n",
      "ODOMETRY -0.0105209157752 0.100056520755 -0.0199923529952\n",
      "\n",
      "sqP = \n",
      "[[0.4180760916091149, 0.0, 0.0], [0.006270318239285371, 0.29624038363661787, 0.0], [-0.014388450865495832, 0.0027983984472574413, 0.26131271740376827]]\n",
      "sigma = \n",
      "[[ 0.6131384   1.91179178  8.34961331]\n",
      " [ 1.44929059  1.91179178  8.34961331]\n",
      " [ 0.62567904  2.50427255  8.34961331]\n",
      " [ 0.5843615   1.91738858  8.87223874]\n",
      " [-0.22301378  1.91179178  8.34961331]\n",
      " [ 0.60059777  1.31931101  8.34961331]\n",
      " [ 0.6419153   1.90619498  7.82698787]]\n",
      "sqP = \n",
      "[[0.6124001655651995, 0.0, 0.0], [-0.011424700948051156, 0.5366669098194723, 0.0], [0.014752180360876319, 0.001329597931272942, 0.5181182451497437]]\n",
      "m = \n",
      "[0.58256735 1.83385352 8.46553104]\n",
      "P = \n",
      "[[ 0.1754393   0.002592   -0.00649175]\n",
      " [ 0.002592    0.08873594 -0.00102468]\n",
      " [-0.00649175 -0.00102468  0.06751632]]\n",
      "num = \n",
      "469\n",
      "SENSOR 9 3.11237333983 -0.828608726065\n",
      "\n",
      "num = \n",
      "470\n",
      "SENSOR 2 5.1348537585 -2.83576534622\n",
      "\n",
      "num = \n",
      "471\n",
      "SENSOR 3 1.7099726366 -2.2858612058\n",
      "\n",
      "num = \n",
      "472\n",
      "SENSOR 7 4.90049538427 -1.78150085169\n",
      "\n",
      "num = \n",
      "473\n",
      "ODOMETRY -0.010155344285 0.100089786643 -0.0198085659559\n",
      "\n",
      "sqP = \n",
      "[[0.41885475076822115, 0.0, 0.0], [0.006188311894075708, 0.2978215002308504, 0.0], [-0.015498820427599907, -0.0031185428667259347, 0.2593576290249027]]\n",
      "sigma = \n",
      "[[ 0.58256735  1.83385352  8.46553104]\n",
      " [ 1.42027685  1.83385352  8.46553104]\n",
      " [ 0.59494397  2.42949652  8.46553104]\n",
      " [ 0.55156971  1.82761643  8.98424629]\n",
      " [-0.25514215  1.83385352  8.46553104]\n",
      " [ 0.57019073  1.23821052  8.46553104]\n",
      " [ 0.61356499  1.8400906   7.94681578]]\n",
      "sqP = \n",
      "[[0.6129582456251437, 0.0, 0.0], [-0.010691567023911868, 0.5375305703180199, 0.0], [0.014839858665931132, -0.001507440924506118, 0.5171139409523258]]\n",
      "m = \n",
      "[0.56278848 1.93804754 8.65401482]\n",
      "P = \n",
      "[[ 0.17554881  0.00224764 -0.00593529]\n",
      " [ 0.00224764  0.08903312  0.00127384]\n",
      " [-0.00593529  0.00127384  0.06703189]]\n",
      "num = \n",
      "474\n",
      "SENSOR 9 2.99996691085 -0.765112595878\n",
      "\n",
      "num = \n",
      "475\n",
      "SENSOR 3 1.6542766528 -2.40104297088\n",
      "\n",
      "num = \n",
      "476\n",
      "SENSOR 7 4.68837972288 -1.72827448368\n",
      "\n",
      "num = \n",
      "477\n",
      "ODOMETRY -0.00967291193757 0.100124175068 -0.0205587676625\n",
      "\n",
      "sqP = \n",
      "[[0.4189854473731938, 0.0, 0.0], [0.005364475491612525, 0.2983359561294078, 0.0], [-0.014165852177737718, 0.004524535691029194, 0.2584777475690871]]\n",
      "sigma = \n",
      "[[ 0.56278848  1.93804754  8.65401482]\n",
      " [ 1.40075937  1.93804754  8.65401482]\n",
      " [ 0.57351743  2.53471946  8.65401482]\n",
      " [ 0.53445677  1.94709661  9.17097031]\n",
      " [-0.27518242  1.93804754  8.65401482]\n",
      " [ 0.55205953  1.34137563  8.65401482]\n",
      " [ 0.59112018  1.92899847  8.13705932]]\n",
      "sqP = \n",
      "[[0.6130075481392844, 0.0, 0.0], [-0.010859459960277951, 0.5378441534476877, 0.0], [0.01569857979268471, 0.0021118393293658045, 0.5167173197990538]]\n",
      "m = \n",
      "[0.54403333 2.08262441 8.63563872]\n",
      "P = \n",
      "[[ 0.18149291  0.00136969 -0.0064997 ]\n",
      " [ 0.00136969  0.10001946  0.01709231]\n",
      " [-0.0064997   0.01709231  0.09170124]]\n",
      "num = \n",
      "478\n",
      "SENSOR 9 2.99807347078 -0.696237174014\n",
      "\n",
      "num = \n",
      "479\n",
      "SENSOR 3 1.7267487227 -2.3662440395\n",
      "\n",
      "num = \n",
      "480\n",
      "SENSOR 7 4.7350984348 -1.75139975295\n",
      "\n",
      "num = \n",
      "481\n",
      "ODOMETRY -0.0101662059319 0.100228934457 -0.0199252685348\n",
      "\n",
      "sqP = \n",
      "[[0.42601984459496245, 0.0, 0.0], [0.003215083369212161, 0.31624218798461684, 0.0], [-0.015256802144110009, 0.054203262104607644, 0.2975407203425855]]\n",
      "sigma = \n",
      "[[ 0.54403333  2.08262441  8.63563872]\n",
      " [ 1.39607302  2.08262441  8.63563872]\n",
      " [ 0.5504635   2.71510879  8.63563872]\n",
      " [ 0.51351972  2.19103094  9.23072016]\n",
      " [-0.30800636  2.08262441  8.63563872]\n",
      " [ 0.53760316  1.45014004  8.63563872]\n",
      " [ 0.57454693  1.97421789  8.04055728]]\n",
      "sqP = \n",
      "[[0.617847889663347, 0.0, 0.0], [-0.012944176513480487, 0.5507188887717528, 0.0], [0.015044666082811882, 0.029130942207446688, 0.5364544496771274]]\n",
      "m = \n",
      "[0.53046829 2.12743321 8.6738493 ]\n",
      "P = \n",
      "[[ 0.18374472  0.00075685 -0.00810764]\n",
      " [ 0.00075685  0.10161376  0.01633977]\n",
      " [-0.00810764  0.01633977  0.09405726]]\n",
      "num = \n",
      "482\n",
      "SENSOR 9 3.15312197773 -0.769591095276\n",
      "\n",
      "num = \n",
      "483\n",
      "SENSOR 3 1.73627144755 -2.31623144182\n",
      "\n",
      "num = \n",
      "484\n",
      "SENSOR 7 5.06411728014 -1.71705818396\n",
      "\n",
      "num = \n",
      "485\n",
      "ODOMETRY -0.0107211355799 0.0999927335192 -0.020501983192\n",
      "\n",
      "sqP = \n",
      "[[0.42865454788976437, 0.0, 0.0], [0.0017656392360219375, 0.31876424083404553, 0.0], [-0.0189141550810327, 0.05136449813578445, 0.3017634807039834]]\n",
      "sigma = \n",
      "[[ 0.53046829  2.12743321  8.6738493 ]\n",
      " [ 1.38777739  2.12743321  8.6738493 ]\n",
      " [ 0.53399957  2.76496169  8.6738493 ]\n",
      " [ 0.49263998  2.23016221  9.27737627]\n",
      " [-0.32684081  2.12743321  8.6738493 ]\n",
      " [ 0.52693701  1.48990473  8.6738493 ]\n",
      " [ 0.5682966   2.02470421  8.07032234]]\n",
      "sqP = \n",
      "[[0.6197625223994425, 0.0, 0.0], [-0.013716991450939863, 0.5519296144385647, 0.0], [0.013580190765168112, 0.027922078719881335, 0.5387427442102353]]\n",
      "m = \n",
      "[0.5254687  1.98613315 8.77922972]\n",
      "P = \n",
      "[[ 0.18471989  0.00044307 -0.00870659]\n",
      " [ 0.00044307  0.10210845  0.01578613]\n",
      " [-0.00870659  0.01578613  0.09371471]]\n",
      "num = \n",
      "486\n",
      "SENSOR 9 2.86278392437 -0.743762595525\n",
      "\n",
      "num = \n",
      "487\n",
      "SENSOR 3 1.84695799822 -2.43783547802\n",
      "\n",
      "num = \n",
      "488\n",
      "SENSOR 7 5.07231532054 -1.68808003478\n",
      "\n",
      "num = \n",
      "489\n",
      "ODOMETRY -0.00979459746259 0.100025076699 -0.0197251786846\n",
      "\n",
      "sqP = \n",
      "[[0.42979052028578707, 0.0, 0.0], [0.0010308927969269871, 0.31954247034420996, 0.0], [-0.02025775422524488, 0.04946764347197401, 0.3014254204832902]]\n",
      "sigma = \n",
      "[[ 0.5254687   1.98613315  8.77922972]\n",
      " [ 1.38504974  1.98613315  8.77922972]\n",
      " [ 0.52753049  2.62521809  8.77922972]\n",
      " [ 0.48495319  2.08506844  9.38208056]\n",
      " [-0.33411234  1.98613315  8.77922972]\n",
      " [ 0.52340692  1.34704821  8.77922972]\n",
      " [ 0.56598421  1.88719787  8.17637888]]\n",
      "sqP = \n",
      "[[0.6205895025487755, 0.0, 0.0], [-0.014030448005134625, 0.552216878575616, 0.0], [0.013050148389752018, 0.026830232475874342, 0.5385621418628949]]\n",
      "m = \n",
      "[0.48289843 2.10486281 8.88800644]\n",
      "P = \n",
      "[[ 0.18477354 -0.00026071 -0.007911  ]\n",
      " [-0.00026071  0.10343788  0.02047429]\n",
      " [-0.007911    0.02047429  0.09504355]]\n",
      "num = \n",
      "490\n",
      "SENSOR 9 2.82075829413 -0.688148179119\n",
      "\n",
      "num = \n",
      "491\n",
      "SENSOR 3 2.02116189813 -2.39856289262\n",
      "\n",
      "num = \n",
      "492\n",
      "SENSOR 7 4.91405340171 -1.71247547809\n",
      "\n",
      "num = \n",
      "493\n",
      "ODOMETRY -0.00880991332931 0.0997863392491 -0.0197458769482\n",
      "\n",
      "sqP = \n",
      "[[0.42985293083491233, 0.0, 0.0], [-0.0006065177348220215, 0.32161703113768847, 0.0], [-0.018403976051481603, 0.06362575667640104, 0.30109236310390103]]\n",
      "sigma = \n",
      "[[ 0.48289843  2.10486281  8.88800644]\n",
      " [ 1.3426043   2.10486281  8.88800644]\n",
      " [ 0.4816854   2.74809687  8.88800644]\n",
      " [ 0.44609048  2.23211432  9.49019117]\n",
      " [-0.37680743  2.10486281  8.88800644]\n",
      " [ 0.48411147  1.46162875  8.88800644]\n",
      " [ 0.51970639  1.97761129  8.28582171]]\n",
      "sqP = \n",
      "[[0.6205744245017113, 0.0, 0.0], [-0.014185459935638406, 0.5548755924104423, 0.0], [0.014483010834358047, 0.034244622266238346, 0.5380287442967078]]\n",
      "m = \n",
      "[0.44942354 2.22972196 8.95769956]\n",
      "P = \n",
      "[[ 0.18477089 -0.00093553 -0.00797552]\n",
      " [-0.00093553  0.10445053  0.0199374 ]\n",
      " [-0.00797552  0.0199374   0.09389488]]\n",
      "num = \n",
      "494\n",
      "SENSOR 9 2.83728718149 -0.703540952524\n",
      "\n",
      "num = \n",
      "495\n",
      "SENSOR 3 1.93000650123 -2.36970627286\n",
      "\n",
      "num = \n",
      "496\n",
      "SENSOR 7 4.79347993043 -1.6329273271\n",
      "\n",
      "num = \n",
      "497\n",
      "ODOMETRY -0.0100580444163 0.10021748682 -0.0200095441708\n",
      "\n",
      "sqP = \n",
      "[[0.4298498482142991, 0.0, 0.0], [-0.0021764052070627126, 0.3231807381821173, 0.0], [-0.018554210607488006, 0.061566228906957, 0.29960010613116106]]\n",
      "sigma = \n",
      "[[ 0.44942354  2.22972196  8.95769956]\n",
      " [ 1.30912324  2.22972196  8.95769956]\n",
      " [ 0.44507073  2.87608344  8.95769956]\n",
      " [ 0.41231512  2.35285442  9.55689977]\n",
      " [-0.41027616  2.22972196  8.95769956]\n",
      " [ 0.45377635  1.58336048  8.95769956]\n",
      " [ 0.48653196  2.1065895   8.35849935]]\n",
      "sqP = \n",
      "[[0.6205802828649556, 0.0, 0.0], [-0.014165147407340734, 0.5555512481980565, 0.0], [0.014775908760232282, 0.032865940362487356, 0.5372813988786717]]\n",
      "m = \n",
      "[0.42181116 2.28000745 8.90710615]\n",
      "P = \n",
      "[[ 0.18488087 -0.00125891 -0.00830255]\n",
      " [-0.00125891  0.10496867  0.01814418]\n",
      " [-0.00830255  0.01814418  0.09234566]]\n",
      "num = \n",
      "498\n",
      "SENSOR 9 2.51641560778 -0.756300137634\n",
      "\n",
      "num = \n",
      "499\n",
      "SENSOR 3 2.09607292645 -2.38526777678\n",
      "\n",
      "num = \n",
      "500\n",
      "SENSOR 7 5.06835478059 -1.72475485601\n",
      "\n",
      "num = \n",
      "501\n",
      "ODOMETRY -0.0104281508158 0.10000140247 -0.0199377152457\n",
      "\n",
      "sqP = \n",
      "[[0.4299777598744712, 0.0, 0.0], [-0.002927841267803932, 0.32397546192528953, 0.0], [-0.019309246407823374, 0.05583028152666683, 0.2980868867164586]]\n",
      "sigma = \n",
      "[[ 0.42181116  2.28000745  8.90710615]\n",
      " [ 1.28176668  2.28000745  8.90710615]\n",
      " [ 0.41595548  2.92795837  8.90710615]\n",
      " [ 0.38319267  2.39166801  9.50327992]\n",
      " [-0.43814436  2.28000745  8.90710615]\n",
      " [ 0.42766684  1.63205652  8.90710615]\n",
      " [ 0.46042965  2.16834688  8.31093238]]\n",
      "sqP = \n",
      "[[0.6206950082270647, 0.0, 0.0], [-0.01381749808481664, 0.5554017030058839, 0.0], [0.014889954973270498, 0.029610549425162964, 0.5366105638552305]]\n",
      "m = \n",
      "[0.41287108 2.42324081 9.07542522]\n",
      "P = \n",
      "[[ 0.18506009 -0.00081869 -0.00860669]\n",
      " [-0.00081869  0.10419288  0.0159655 ]\n",
      " [-0.00860669  0.0159655   0.09185191]]\n",
      "num = \n",
      "502\n",
      "SENSOR 9 2.45948289837 -0.76021264418\n",
      "\n",
      "num = \n",
      "503\n",
      "SENSOR 3 2.06522117309 -2.33318420312\n",
      "\n",
      "num = \n",
      "504\n",
      "SENSOR 7 4.84613918824 -1.61007685337\n",
      "\n",
      "num = \n",
      "505\n",
      "ODOMETRY -0.0097623943034 0.0999813928244 -0.0206369980195\n",
      "\n",
      "sqP = \n",
      "[[0.4301861069996527, 0.0, 0.0], [-0.001903113414369427, 0.3227836111484724, 0.0], [-0.02000689063856399, 0.04934396116621302, 0.29835685124725664]]\n",
      "sigma = \n",
      "[[ 0.41287108  2.42324081  9.07542522]\n",
      " [ 1.27324329  2.42324081  9.07542522]\n",
      " [ 0.40906485  3.06880803  9.07542522]\n",
      " [ 0.3728573   2.52192873  9.67213892]\n",
      " [-0.44750113  2.42324081  9.07542522]\n",
      " [ 0.41667731  1.77767358  9.07542522]\n",
      " [ 0.45288486  2.32455288  8.47871152]]\n",
      "sqP = \n",
      "[[0.6208574588174068, 0.0, 0.0], [-0.012891688731836189, 0.5540723082871866, 0.0], [0.014610975118417038, 0.026276841115895965, 0.5369070789809954]]\n",
      "m = \n",
      "[0.38995384 2.54912356 9.0495513 ]\n",
      "P = \n",
      "[[ 0.18519238 -0.00158811 -0.00881611]\n",
      " [-0.00158811  0.10561336  0.01543441]\n",
      " [-0.00881611  0.01543441  0.09020296]]\n",
      "num = \n",
      "506\n",
      "SENSOR 9 2.57479230149 -0.632819077978\n",
      "\n",
      "num = \n",
      "507\n",
      "SENSOR 3 2.177346285 -2.42626146831\n",
      "\n",
      "num = \n",
      "508\n",
      "SENSOR 7 5.05551333216 -1.69863156486\n",
      "\n",
      "num = \n",
      "509\n",
      "ODOMETRY -0.00975220094369 0.100042185698 -0.0200047749536\n",
      "\n",
      "sqP = \n",
      "[[0.43033983797032915, 0.0, 0.0], [-0.003690365769912754, 0.32496114369378215, 0.0], [-0.020486395011946445, 0.04726351733383017, 0.29588752583607586]]\n",
      "sigma = \n",
      "[[ 0.38995384  2.54912356  9.0495513 ]\n",
      " [ 1.25063352  2.54912356  9.0495513 ]\n",
      " [ 0.38257311  3.19904585  9.0495513 ]\n",
      " [ 0.34898105  2.64365059  9.64132635]\n",
      " [-0.47072583  2.54912356  9.0495513 ]\n",
      " [ 0.39733458  1.89920127  9.0495513 ]\n",
      " [ 0.43092663  2.45459652  8.45777625]]\n",
      "sqP = \n",
      "[[0.6209876708312576, 0.0, 0.0], [-0.01326480602997375, 0.5551688251903232, 0.0], [0.014697058240545922, 0.024829801149473082, 0.5355984392231995]]\n",
      "m = \n",
      "[0.35491046 2.51083418 9.15429852]\n",
      "P = \n",
      "[[ 0.18537648 -0.00147996 -0.00933515]\n",
      " [-0.00147996  0.1047383   0.01251115]\n",
      " [-0.00933515  0.01251115  0.08977407]]\n",
      "num = \n",
      "510\n",
      "SENSOR 9 2.5074325001 -0.609410447289\n",
      "\n",
      "num = \n",
      "511\n",
      "SENSOR 3 2.3010604256 -2.35179262067\n",
      "\n",
      "num = \n",
      "512\n",
      "SENSOR 7 4.90749675187 -1.70419293696\n",
      "\n",
      "num = \n",
      "513\n",
      "ODOMETRY -0.00927676045743 0.100208064048 -0.0191796860605\n",
      "\n",
      "sqP = \n",
      "[[0.43055368767126, 0.0, 0.0], [-0.0034373442842608208, 0.3236147098512404, 0.0], [-0.021681738234550493, 0.03843032960299375, 0.2963563420557373]]\n",
      "sigma = \n",
      "[[ 0.35491046  2.51083418  9.15429852]\n",
      " [ 1.21601784  2.51083418  9.15429852]\n",
      " [ 0.34803577  3.1580636   9.15429852]\n",
      " [ 0.31154698  2.58769484  9.74701121]\n",
      " [-0.50619691  2.51083418  9.15429852]\n",
      " [ 0.36178515  1.86360476  9.15429852]\n",
      " [ 0.39827394  2.43397352  8.56158584]]\n",
      "sqP = \n",
      "[[0.6211750084122196, 0.0, 0.0], [-0.012056957083971689, 0.5536753637875969, 0.0], [0.014477941133205689, 0.02024719257588593, 0.5360068286645117]]\n",
      "m = \n",
      "[0.32704458 2.58957283 9.18951757]\n",
      "P = \n",
      "[[ 0.18539921 -0.00181273 -0.00896358]\n",
      " [-0.00181273  0.10632923  0.01449542]\n",
      " [-0.00896358  0.01449542  0.08906105]]\n",
      "num = \n",
      "514\n",
      "SENSOR 9 2.41626913018 -0.724594121384\n",
      "\n",
      "num = \n",
      "515\n",
      "SENSOR 3 2.52272282794 -2.4239408799\n",
      "\n",
      "num = \n",
      "516\n",
      "SENSOR 7 4.70640264651 -1.63618162589\n",
      "\n",
      "num = \n",
      "517\n",
      "ODOMETRY -0.0099067860188 0.0998711486494 -0.0194012350076\n",
      "\n",
      "sqP = \n",
      "[[0.4305800811757691, 0.0, 0.0], [-0.004209965675518057, 0.3260544563243401, 0.0], [-0.020817460628455707, 0.04418827299916473, 0.29440632548143325]]\n",
      "sigma = \n",
      "[[ 0.32704458  2.58957283  9.18951757]\n",
      " [ 1.18820474  2.58957283  9.18951757]\n",
      " [ 0.31862465  3.24168174  9.18951757]\n",
      " [ 0.28540966  2.67794937  9.77833022]\n",
      " [-0.53411559  2.58957283  9.18951757]\n",
      " [ 0.33546451  1.93746391  9.18951757]\n",
      " [ 0.3686795   2.50119628  8.60070492]]\n",
      "sqP = \n",
      "[[0.6211684930703893, 0.0, 0.0], [-0.011945467641651141, 0.5555289005980941, 0.0], [0.01523039904227214, 0.023045558320231356, 0.5348603075079431]]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "m = \n",
      "[0.30592627 2.74876029 9.23013502]\n",
      "P = \n",
      "[[ 0.18536767 -0.00209989 -0.00896446]\n",
      " [-0.00209989  0.10664609  0.01392246]\n",
      " [-0.00896446  0.01392246  0.08870398]]\n",
      "num = \n",
      "518\n",
      "SENSOR 9 2.24529394017 -0.685502521964\n",
      "\n",
      "num = \n",
      "519\n",
      "SENSOR 3 2.31844097149 -2.42828855555\n",
      "\n",
      "num = \n",
      "520\n",
      "SENSOR 7 4.95528000197 -1.57288604154\n",
      "\n",
      "num = \n",
      "521\n",
      "ODOMETRY -0.00983384980744 0.0999224766995 -0.0204305528243\n",
      "\n",
      "sqP = \n",
      "[[0.43054346119830195, 0.0, 0.0], [-0.004877307976330142, 0.3265306993951554, 0.0], [-0.020821271196990173, 0.042326522989220475, 0.29407298072546056]]\n",
      "sigma = \n",
      "[[ 0.30592627  2.74876029  9.23013502]\n",
      " [ 1.16701319  2.74876029  9.23013502]\n",
      " [ 0.29617166  3.40182169  9.23013502]\n",
      " [ 0.26428373  2.83341333  9.81828098]\n",
      " [-0.55516065  2.74876029  9.23013502]\n",
      " [ 0.31568089  2.09569889  9.23013502]\n",
      " [ 0.34756881  2.66410724  8.64198905]]\n",
      "sqP = \n",
      "[[0.6211481187671491, 0.0, 0.0], [-0.011663267446931917, 0.5556543242266077, 0.0], [0.015326124258030857, 0.022009159018479826, 0.5347207607423746]]\n",
      "m = \n",
      "[0.26402828 2.80198652 9.25006914]\n",
      "P = \n",
      "[[ 0.18543306 -0.00244061 -0.00937959]\n",
      " [-0.00244061  0.10652169  0.01179788]\n",
      " [-0.00937959  0.01179788  0.0880935 ]]\n",
      "num = \n",
      "522\n",
      "SENSOR 9 2.18268166452 -0.647873105786\n",
      "\n",
      "num = \n",
      "523\n",
      "SENSOR 3 2.60693896315 -2.37482011041\n",
      "\n",
      "num = \n",
      "524\n",
      "SENSOR 7 4.87407767625 -1.66971167459\n",
      "\n",
      "num = \n",
      "525\n",
      "ODOMETRY -0.0102628286794 0.0997605730161 -0.0210500632473\n",
      "\n",
      "sqP = \n",
      "[[0.43061939599833676, 0.0, 0.0], [-0.005667668423640893, 0.32632739823498214, 0.0], [-0.0217816290060459, 0.03577520177208409, 0.29383531695733045]]\n",
      "sigma = \n",
      "[[ 0.26402828  2.80198652  9.25006914]\n",
      " [ 1.12526707  2.80198652  9.25006914]\n",
      " [ 0.25269294  3.45464132  9.25006914]\n",
      " [ 0.22046502  2.87353692  9.83773978]\n",
      " [-0.59721051  2.80198652  9.25006914]\n",
      " [ 0.27536362  2.14933172  9.25006914]\n",
      " [ 0.30759154  2.73043612  8.66239851]]\n",
      "sqP = \n",
      "[[0.621240393114847, 0.0, 0.0], [-0.010858694916514512, 0.5550561823777016, 0.0], [0.015205214915508861, 0.01853512265415866, 0.5346884799895483]]\n",
      "m = \n",
      "[0.24084243 2.93174023 9.34299857]\n",
      "P = \n",
      "[[ 0.18553092 -0.00240048 -0.0095421 ]\n",
      " [-0.00240048  0.10649881  0.01090663]\n",
      " [-0.0095421   0.01090663  0.08775758]]\n",
      "num = \n",
      "526\n",
      "SENSOR 9 2.15683089899 -0.642555173461\n",
      "\n",
      "num = \n",
      "527\n",
      "SENSOR 3 2.6756869293 -2.29039615081\n",
      "\n",
      "num = \n",
      "528\n",
      "SENSOR 7 5.11674482285 -1.47951929626\n",
      "\n",
      "num = \n",
      "529\n",
      "ODOMETRY -0.0101962211399 0.099836539792 -0.0188237479265\n",
      "\n",
      "sqP = \n",
      "[[0.4307329994425358, 0.0, 0.0], [-0.005573022263666844, 0.3262939685485146, 0.0], [-0.02215316348244515, 0.03304742295720525, 0.29355525311383196]]\n",
      "sigma = \n",
      "[[ 0.24084243  2.93174023  9.34299857]\n",
      " [ 1.10230843  2.93174023  9.34299857]\n",
      " [ 0.22969639  3.58432817  9.34299857]\n",
      " [ 0.1965361   2.99783507  9.93010907]\n",
      " [-0.62062357  2.93174023  9.34299857]\n",
      " [ 0.25198848  2.27915229  9.34299857]\n",
      " [ 0.28514876  2.86564538  8.75588806]]\n",
      "sqP = \n",
      "[[0.6213314236687171, 0.0, 0.0], [-0.010120724146499982, 0.5548544185698621, 0.0], [0.015159158175117274, 0.017095818042346188, 0.534569288523036]]\n",
      "m = \n",
      "[0.17909686 2.96497611 9.48890549]\n",
      "P = \n",
      "[[ 0.18558713 -0.00317574 -0.009698  ]\n",
      " [-0.00317574  0.10779647  0.01039606]\n",
      " [-0.009698    0.01039606  0.08681845]]\n",
      "num = \n",
      "530\n",
      "SENSOR 9 1.9915555629 -0.605343778173\n",
      "\n",
      "num = \n",
      "531\n",
      "SENSOR 3 2.8321255953 -2.36581198314\n",
      "\n",
      "num = \n",
      "532\n",
      "SENSOR 7 4.93146123799 -1.5890166699\n",
      "\n",
      "num = \n",
      "533\n",
      "ODOMETRY -0.0109072611821 0.100057984487 -0.0199246493126\n",
      "\n",
      "sqP = \n",
      "[[0.43079824249262616, 0.0, 0.0], [-0.007371757429086579, 0.3282409621896817, 0.0], [-0.02251170764711429, 0.031166468053317763, 0.29213066153842576]]\n",
      "sigma = \n",
      "[[ 0.17909686  2.96497611  9.48890549]\n",
      " [ 1.04069334  2.96497611  9.48890549]\n",
      " [ 0.16435334  3.62145804  9.48890549]\n",
      " [ 0.13407344  3.02730905 10.07316681]\n",
      " [-0.68249963  2.96497611  9.48890549]\n",
      " [ 0.19384037  2.30849419  9.48890549]\n",
      " [ 0.22412027  2.90264318  8.90464416]]\n",
      "sqP = \n",
      "[[0.621408276033976, 0.0, 0.0], [-0.009452777465787814, 0.5558687133762201, 0.0], [0.015394354411983002, 0.015925055506624743, 0.5338183940487234]]\n",
      "m = \n",
      "[0.12703178 3.14424972 9.52089199]\n",
      "P = \n",
      "[[ 0.18557046 -0.00424238 -0.00932008]\n",
      " [-0.00424238  0.11142324  0.01228958]\n",
      " [-0.00932008  0.01228958  0.08536851]]\n",
      "num = \n",
      "534\n",
      "SENSOR 9 2.05060986776 -0.812359999266\n",
      "\n",
      "num = \n",
      "535\n",
      "SENSOR 3 2.89469192638 -2.33538439474\n",
      "\n",
      "num = \n",
      "536\n",
      "SENSOR 7 5.06379731865 -1.57967642574\n",
      "\n",
      "num = \n",
      "537\n",
      "ODOMETRY -0.0110088218391 0.10014697699 -0.0202732496012\n",
      "\n",
      "sqP = \n",
      "[[0.4307788960744924, 0.0, 0.0], [-0.009848172690576276, 0.3336558860443832, 0.0], [-0.021635407830646, 0.03619450753767359, 0.2891200077993833]]\n",
      "sigma = \n",
      "[[ 0.12703178  3.14424972  9.52089199]\n",
      " [ 0.98858957  3.14424972  9.52089199]\n",
      " [ 0.10733543  3.81156149  9.52089199]\n",
      " [ 0.08376096  3.21663874 10.09913201]\n",
      " [-0.73452601  3.14424972  9.52089199]\n",
      " [ 0.14672812  2.47693795  9.52089199]\n",
      " [ 0.17030259  3.07186071  8.94265198]]\n",
      "sqP = \n",
      "[[0.6213980485007176, 0.0, 0.0], [-0.009608049791485317, 0.5593590018577136, 0.0], [0.01616556109043832, 0.018141689125082995, 0.5321537383285766]]\n",
      "m = \n",
      "[0.11970666 3.18675642 9.61410296]\n",
      "P = \n",
      "[[ 0.18558431 -0.00529554 -0.00958094]\n",
      " [-0.00529554  0.11256308  0.01051946]\n",
      " [-0.00958094  0.01051946  0.0848536 ]]\n",
      "num = \n",
      "538\n",
      "SENSOR 9 1.9855033654 -0.729611380785\n",
      "\n",
      "num = \n",
      "539\n",
      "SENSOR 3 2.808098691 -2.40456966848\n",
      "\n",
      "num = \n",
      "540\n",
      "SENSOR 7 5.00506879142 -1.52174912725\n",
      "\n",
      "num = \n",
      "541\n",
      "ODOMETRY -0.0097077990525 0.100157141283 -0.0214692363299\n",
      "\n",
      "sqP = \n",
      "[[0.43079497485572293, 0.0, 0.0], [-0.012292475867762626, 0.33527895631798904, 0.0], [-0.022240133242911114, 0.030559842360936902, 0.28883399271661536]]\n",
      "sigma = \n",
      "[[ 0.11970666  3.18675642  9.61410296]\n",
      " [ 0.98129661  3.18675642  9.61410296]\n",
      " [ 0.09512171  3.85731433  9.61410296]\n",
      " [ 0.07522639  3.24787611 10.19177094]\n",
      " [-0.74188329  3.18675642  9.61410296]\n",
      " [ 0.14429161  2.51619851  9.61410296]\n",
      " [ 0.16418693  3.12563674  9.03643497]]\n",
      "sqP = \n",
      "[[0.6214740854215604, 0.0, 0.0], [-0.01059411517879075, 0.5599826071943058, 0.0], [0.0159524523921699, 0.015100049319489049, 0.5320714568904789]]\n",
      "m = \n",
      "[0.07841708 3.22459243 9.58270225]\n",
      "P = \n",
      "[[ 0.18569938 -0.00647774 -0.00949478]\n",
      " [-0.00647774  0.11594924  0.01122448]\n",
      " [-0.00949478  0.01122448  0.08364753]]\n",
      "num = \n",
      "542\n",
      "SENSOR 9 1.85666944851 -0.732607970484\n",
      "\n",
      "num = \n",
      "543\n",
      "SENSOR 3 3.15325419611 -2.35565991968\n",
      "\n",
      "num = \n",
      "544\n",
      "SENSOR 7 4.77716786006 -1.61242538926\n",
      "\n",
      "num = \n",
      "545\n",
      "ODOMETRY -0.00981948245873 0.0999149565253 -0.020711936978\n",
      "\n",
      "sqP = \n",
      "[[0.43092850515711, 0.0, 0.0], [-0.015032054234666279, 0.3401812367726661, 0.0], [-0.022033315012331717, 0.032021960768731886, 0.28659492963498295]]\n",
      "sigma = \n",
      "[[ 0.07841708  3.22459243  9.58270225]\n",
      " [ 0.94027409  3.22459243  9.58270225]\n",
      " [ 0.04835297  3.9049549   9.58270225]\n",
      " [ 0.03435045  3.28863635 10.15589211]\n",
      " [-0.78343993  3.22459243  9.58270225]\n",
      " [ 0.10848119  2.54422995  9.58270225]\n",
      " [ 0.12248371  3.1605485   9.0095124 ]]\n",
      "sqP = \n",
      "[[0.6216195027361093, 0.0, 0.0], [-0.011178342450958095, 0.562972417999661, 0.0], [0.01624523647790747, 0.015519706458053248, 0.5308640735407051]]\n",
      "m = \n",
      "[0.05867838 3.43556891 9.61104099]\n",
      "P = \n",
      "[[ 0.18574191 -0.00655285 -0.00962553]\n",
      " [-0.00655285  0.11532427  0.0102423 ]\n",
      " [-0.00962553  0.0102423   0.08394767]]\n",
      "num = \n",
      "546\n",
      "SENSOR 9 1.66695280875 -0.677940267447\n",
      "\n",
      "num = \n",
      "547\n",
      "SENSOR 3 2.93583551959 -2.29721883235\n",
      "\n",
      "num = \n",
      "548\n",
      "SENSOR 7 4.77792363509 -1.55059855566\n",
      "\n",
      "num = \n",
      "549\n",
      "ODOMETRY -0.00992896039159 0.0998189682803 -0.0204511862369\n",
      "\n",
      "sqP = \n",
      "[[0.4309778557773943, 0.0, 0.0], [-0.015204610018559042, 0.33925372318449704, 0.0], [-0.022334172272969816, 0.02918970763941336, 0.2873966205080692]]\n",
      "sigma = \n",
      "[[ 0.05867838  3.43556891  9.61104099]\n",
      " [ 0.92063409  3.43556891  9.61104099]\n",
      " [ 0.02826916  4.11407635  9.61104099]\n",
      " [ 0.01401003  3.49394832 10.18583423]\n",
      " [-0.80327733  3.43556891  9.61104099]\n",
      " [ 0.0890876   2.75706146  9.61104099]\n",
      " [ 0.10334672  3.37718949  9.03624775]]\n",
      "sqP = \n",
      "[[0.6216686477436478, 0.0, 0.0], [-0.010635814945481692, 0.5622446280794101, 0.0], [0.01605160805780331, 0.014143152227049335, 0.5313200594304089]]\n",
      "m = \n",
      "[0.02753623 3.54345548 9.52333991]\n",
      "P = \n",
      "[[ 0.18609985 -0.00827528 -0.01019376]\n",
      " [-0.00827528  0.11750266  0.00751779]\n",
      " [-0.01019376  0.00751779  0.0833813 ]]\n",
      "num = \n",
      "550\n",
      "SENSOR 9 1.71819656468 -0.707245929411\n",
      "\n",
      "num = \n",
      "551\n",
      "SENSOR 3 3.02406315637 -2.3968858507\n",
      "\n",
      "num = \n",
      "552\n",
      "SENSOR 7 4.90173372979 -1.65060778383\n",
      "\n",
      "num = \n",
      "553\n",
      "ODOMETRY -0.00884110158486 0.100015505449 -0.0206146468981\n",
      "\n",
      "sqP = \n",
      "[[0.4313929126398217, 0.0, 0.0], [-0.01918269544234231, 0.34224944926915174, 0.0], [-0.02362987733125194, 0.020641394755179232, 0.28704854352016296]]\n",
      "sigma = \n",
      "[[ 0.02753623  3.54345548  9.52333991]\n",
      " [ 0.89032206  3.54345548  9.52333991]\n",
      " [-0.01082916  4.22795438  9.52333991]\n",
      " [-0.01972352  3.58473827 10.097437  ]\n",
      " [-0.83524959  3.54345548  9.52333991]\n",
      " [ 0.06590162  2.85895658  9.52333991]\n",
      " [ 0.07479599  3.50217269  8.94924282]]\n",
      "sqP = \n",
      "[[0.6221142917366491, 0.0, 0.0], [-0.011802679319699012, 0.5636259759470832, 0.0], [0.015532257422665114, 0.00957216947910511, 0.5311844305029182]]\n",
      "m = \n",
      "[0.01727736 3.57259603 9.58101523]\n",
      "P = \n",
      "[[ 0.18644546 -0.0087255  -0.01093625]\n",
      " [-0.0087255   0.11524228  0.00397146]\n",
      " [-0.01093625  0.00397146  0.08459879]]\n",
      "num = \n",
      "554\n",
      "SENSOR 9 1.70981171667 -0.684234093975\n",
      "\n",
      "num = \n",
      "555\n",
      "SENSOR 3 3.16299858885 -2.37554434755\n",
      "\n",
      "num = \n",
      "556\n",
      "SENSOR 7 5.05446186142 -1.55954853363\n",
      "\n",
      "num = \n",
      "557\n",
      "ODOMETRY -0.0102024861503 0.10032005589 -0.0183984058531\n",
      "\n",
      "sqP = \n",
      "[[0.43179330640174934, 0.0, 0.0], [-0.020207581381507123, 0.33887155336540287, 0.0], [-0.025327509569847518, 0.01020934294075962, 0.28957396142504194]]\n",
      "sigma = \n",
      "[[ 0.01727736  3.57259603  9.58101523]\n",
      " [ 0.88086397  3.57259603  9.58101523]\n",
      " [-0.0231378   4.25033914  9.58101523]\n",
      " [-0.03337766  3.59301472 10.16016315]\n",
      " [-0.84630925  3.57259603  9.58101523]\n",
      " [ 0.05769252  2.89485292  9.58101523]\n",
      " [ 0.06793238  3.55217734  9.00186731]]\n",
      "sqP = \n",
      "[[0.6224911955519178, 0.0, 0.0], [-0.011639863773782793, 0.5612877748363654, 0.0], [0.01475476757643815, 0.004299791823377202, 0.5325467419784427]]\n",
      "m = \n",
      "[-0.02659042  3.61122985  9.70451702]\n",
      "P = \n",
      "[[ 0.18692647 -0.00945266 -0.0110707 ]\n",
      " [-0.00945266  0.11763794  0.0039288 ]\n",
      " [-0.0110707   0.0039288   0.08372376]]\n",
      "num = \n",
      "558\n",
      "SENSOR 9 1.48285099536 -0.732863940447\n",
      "\n",
      "num = \n",
      "559\n",
      "SENSOR 3 3.24066838872 -2.3472192049\n",
      "\n",
      "num = \n",
      "560\n",
      "SENSOR 7 5.02189688945 -1.52629273662\n",
      "\n",
      "num = \n",
      "561\n",
      "ODOMETRY -0.0095354970248 0.100025384548 -0.0207858100942\n",
      "\n",
      "sqP = \n",
      "[[0.43234993689544177, 0.0, 0.0], [-0.021863452560254532, 0.3422863288870911, 0.0], [-0.025605870650432332, 0.009842539899301854, 0.2880472581905927]]\n",
      "sigma = \n",
      "[[-0.02659042  3.61122985  9.70451702]\n",
      " [ 0.83810945  3.61122985  9.70451702]\n",
      " [-0.07031733  4.29580251  9.70451702]\n",
      " [-0.07780216  3.63091493 10.28061154]\n",
      " [-0.8912903   3.61122985  9.70451702]\n",
      " [ 0.01713648  2.9266572   9.70451702]\n",
      " [ 0.02462132  3.59154477  9.1284225 ]]\n",
      "sqP = \n",
      "[[0.6229447319811876, 0.0, 0.0], [-0.011436661321996893, 0.5632958970867457, 0.0], [0.014813755392795102, 0.003989950004694027, 0.5317180399034976]]\n",
      "m = \n",
      "[-0.07820929  3.77645384  9.72747902]\n",
      "P = \n",
      "[[ 0.18749149 -0.01096556 -0.01069075]\n",
      " [-0.01096556  0.12407969  0.00515832]\n",
      " [-0.01069075  0.00515832  0.08173498]]\n",
      "num = \n",
      "562\n",
      "SENSOR 9 1.46150131465 -0.767666096771\n",
      "\n",
      "num = \n",
      "563\n",
      "SENSOR 3 3.22029667183 -2.36535752613\n",
      "\n",
      "num = \n",
      "564\n",
      "SENSOR 7 4.84479242128 -1.46298563412\n",
      "\n",
      "num = \n",
      "565\n",
      "ODOMETRY -0.00906969760687 0.099796851015 -0.0208710090182\n",
      "\n",
      "sqP = \n",
      "[[0.4330028775023266, 0.0, 0.0], [-0.025324460694177278, 0.3513379632830659, 0.0], [-0.024689796028701305, 0.012902291054533332, 0.2845328187815122]]\n",
      "sigma = \n",
      "[[-0.07820929  3.77645384  9.72747902]\n",
      " [ 0.78779647  3.77645384  9.72747902]\n",
      " [-0.12885821  4.47912976  9.72747902]\n",
      " [-0.12758888  3.80225842 10.29654466]\n",
      " [-0.94421504  3.77645384  9.72747902]\n",
      " [-0.02756037  3.07377791  9.72747902]\n",
      " [-0.02882969  3.75064925  9.15841338]]\n",
      "sqP = \n",
      "[[0.6234921060147312, 0.0, 0.0], [-0.012445140620310557, 0.5687964720366021, 0.0], [0.015288396058481367, 0.005255876632106103, 0.5298609231405326]]\n",
      "m = \n",
      "[-0.12196167  3.87946329  9.65811721]\n",
      "P = \n",
      "[[ 0.18857402 -0.01365715 -0.01078053]\n",
      " [-0.01365715  0.13022834  0.0024464 ]\n",
      " [-0.01078053  0.0024464   0.08119311]]\n",
      "num = \n",
      "566\n",
      "SENSOR 9 1.53472735309 -0.701353506585\n",
      "\n",
      "num = \n",
      "567\n",
      "SENSOR 3 3.30563763836 -2.28920291979\n",
      "\n",
      "num = \n",
      "568\n",
      "SENSOR 7 5.08447465054 -1.48707316086\n",
      "\n",
      "num = \n",
      "569\n",
      "ODOMETRY -0.00996926665775 0.100146565387 -0.018774822561\n",
      "\n",
      "sqP = \n",
      "[[0.4342511050706577, 0.0, 0.0], [-0.031449884780174485, 0.35949860268991723, 0.0], [-0.02482556164862842, 0.004633238192794689, 0.2838227133845854]]\n",
      "sigma = \n",
      "[[-0.12196167  3.87946329  9.65811721]\n",
      " [ 0.74654054  3.87946329  9.65811721]\n",
      " [-0.18486143  4.59846049  9.65811721]\n",
      " [-0.17161279  3.88872976 10.22576264]\n",
      " [-0.99046388  3.87946329  9.65811721]\n",
      " [-0.0590619   3.16046608  9.65811721]\n",
      " [-0.07231054  3.87019681  9.09047179]]\n",
      "sqP = \n",
      "[[0.6246434391066182, 0.0, 0.0], [-0.014791021427837524, 0.5735834749499551, 0.0], [0.015227524779149846, 0.0008351017009904462, 0.5295043253629719]]\n",
      "m = \n",
      "[-0.16179095  3.88316203  9.76673293]\n",
      "P = \n",
      "[[ 0.18981413 -0.01531537 -0.01124117]\n",
      " [-0.01531537  0.13115206 -0.00153048]\n",
      " [-0.01124117 -0.00153048  0.08261806]]\n",
      "num = \n",
      "570\n",
      "SENSOR 9 1.27538476788 -0.825565964241\n",
      "\n",
      "num = \n",
      "571\n",
      "SENSOR 3 3.52081304084 -2.35140411346\n",
      "\n",
      "num = \n",
      "572\n",
      "SENSOR 7 4.9713536015 -1.50506099629\n",
      "\n",
      "num = \n",
      "573\n",
      "ODOMETRY -0.00982937584718 0.100296415285 -0.0202122708831\n",
      "\n",
      "sqP = \n",
      "[[0.43567663318282124, 0.0, 0.0], [-0.03515305730397403, 0.3604390755988182, 0.0], [-0.025801631652476162, -0.0067625335872123805, 0.2861933062994242]]\n",
      "sigma = \n",
      "[[-0.16179095  3.88316203  9.76673293]\n",
      " [ 0.70956231  3.88316203  9.76673293]\n",
      " [-0.23209707  4.60404018  9.76673293]\n",
      " [-0.21339422  3.86963697 10.33911954]\n",
      " [-1.03314422  3.88316203  9.76673293]\n",
      " [-0.09148484  3.16228388  9.76673293]\n",
      " [-0.11018769  3.8966871   9.19434632]]\n",
      "sqP = \n",
      "[[0.6258718641505345, 0.0, 0.0], [-0.015355116805332015, 0.5740838726948816, 0.0], [0.014781044896576678, -0.0049752679980022375, 0.5307140626512585]]\n",
      "m = \n",
      "[-0.20812963  4.11540292  9.78049171]\n",
      "P = \n",
      "[[ 0.19066958 -0.0161072  -0.01101952]\n",
      " [-0.0161072   0.13673351 -0.0005728 ]\n",
      " [-0.01101952 -0.0005728   0.08065341]]\n",
      "num = \n",
      "574\n",
      "SENSOR 9 1.28245800998 -0.820894690375\n",
      "\n",
      "num = \n",
      "575\n",
      "SENSOR 3 3.61786607428 -2.28348332028\n",
      "\n",
      "num = \n",
      "576\n",
      "SENSOR 7 4.72683380487 -1.5245034595\n",
      "\n",
      "num = \n",
      "577\n",
      "ODOMETRY -0.00967296493409 0.100095993623 -0.0208377359394\n",
      "\n",
      "sqP = \n",
      "[[0.4366572837711614, 0.0, 0.0], [-0.03688750864584463, 0.3679304553642656, 0.0], [-0.025236077057887534, -0.004086889645556615, 0.28284244485586146]]\n",
      "sigma = \n",
      "[[-0.20812963  4.11540292  9.78049171]\n",
      " [ 0.66518494  4.11540292  9.78049171]\n",
      " [-0.28190465  4.85126383  9.78049171]\n",
      " [-0.25860178  4.10722914 10.3461766 ]\n",
      " [-1.0814442   4.11540292  9.78049171]\n",
      " [-0.13435461  3.37954201  9.78049171]\n",
      " [-0.15765748  4.1235767   9.21480682]]\n",
      "sqP = \n",
      "[[0.6266315754490651, 0.0, 0.0], [-0.015633357651374715, 0.5786707239717578, 0.0], [0.015071379670300183, -0.003676050087516036, 0.5289617054513603]]\n",
      "m = \n",
      "[-0.23481373  4.29732072  9.71307975]\n",
      "P = \n",
      "[[ 0.19332055 -0.01985101 -0.01098205]\n",
      " [-0.01985101  0.14826189 -0.00689524]\n",
      " [-0.01098205 -0.00689524  0.08042766]]\n",
      "num = \n",
      "578\n",
      "SENSOR 9 1.22717122688 -0.834555116798\n",
      "\n",
      "num = \n",
      "579\n",
      "SENSOR 3 3.46599411335 -2.26643278842\n",
      "\n",
      "num = \n",
      "580\n",
      "SENSOR 7 4.77048897684 -1.61915796579\n",
      "\n",
      "num = \n",
      "581\n",
      "ODOMETRY -0.00983012240915 0.100048682167 -0.0200418409326\n",
      "\n",
      "sqP = \n",
      "[[0.4396823298713812, 0.0, 0.0], [-0.04514853137698114, 0.3823918193753262, 0.0], [-0.02497723455567599, -0.020980895382169833, 0.2817154605602652]]\n",
      "sigma = \n",
      "[[-0.23481373  4.29732072  9.71307975]\n",
      " [ 0.64455093  4.29732072  9.71307975]\n",
      " [-0.3251108   5.06210436  9.71307975]\n",
      " [-0.2847682   4.25535893 10.27651067]\n",
      " [-1.11417839  4.29732072  9.71307975]\n",
      " [-0.14451667  3.53253708  9.71307975]\n",
      " [-0.18485926  4.33928251  9.14964883]]\n",
      "sqP = \n",
      "[[0.629271645104619, 0.0, 0.0], [-0.019979820252211233, 0.5880246971273175, 0.0], [0.015365464619432851, -0.012073722406481368, 0.5282558125149378]]\n",
      "m = \n",
      "[-0.22271668  4.33502793  9.65559823]\n",
      "P = \n",
      "[[ 0.19746142 -0.02310148 -0.01134685]\n",
      " [-0.02310148  0.15765468 -0.0162599 ]\n",
      " [-0.01134685 -0.0162599   0.08306531]]\n",
      "num = \n",
      "582\n",
      "SENSOR 9 1.21052101312 -0.846681880547\n",
      "\n",
      "num = \n",
      "583\n",
      "SENSOR 3 3.70451197871 -2.25861912203\n",
      "\n",
      "num = \n",
      "584\n",
      "SENSOR 7 5.07806532139 -1.49123903126\n",
      "\n",
      "num = \n",
      "585\n",
      "ODOMETRY -0.00930456459578 0.100071988622 -0.0204347040085\n",
      "\n",
      "sqP = \n",
      "[[0.4443663166651511, 0.0, 0.0], [-0.0519874718061813, 0.3936394038398497, 0.0], [-0.025534904928240964, -0.0446789577122767, 0.28357903138785606]]\n",
      "sigma = \n",
      "[[-0.22271668  4.33502793  9.65559823]\n",
      " [ 0.66601596  4.33502793  9.65559823]\n",
      " [-0.32669162  5.12230674  9.65559823]\n",
      " [-0.27378649  4.24567001 10.22275629]\n",
      " [-1.11144931  4.33502793  9.65559823]\n",
      " [-0.11874173  3.54774912  9.65559823]\n",
      " [-0.17164687  4.42438584  9.08844016]]\n",
      "sqP = \n",
      "[[0.6331004280373266, 0.0, 0.0], [-0.024118578924155004, 0.5964887523014073, 0.0], [0.01559995504717502, -0.023553665955821257, 0.5288503033441033]]\n",
      "m = \n",
      "[-0.27729309  4.4097446   9.81672569]\n",
      "P = \n",
      "[[ 0.20036122 -0.02436964 -0.01162978]\n",
      " [-0.02436964  0.16058015 -0.02128267]\n",
      " [-0.01162978 -0.02128267  0.08571429]]\n",
      "num = \n",
      "586\n",
      "SENSOR 9 1.12120365478 -0.935390860361\n",
      "\n",
      "num = \n",
      "587\n",
      "SENSOR 3 3.58620574347 -2.27331836708\n",
      "\n",
      "num = \n",
      "588\n",
      "SENSOR 7 4.76861242282 -1.53629792407\n",
      "\n",
      "num = \n",
      "589\n",
      "ODOMETRY 0.000468699390922 0.0998304456128 -2.5849995276e-05\n",
      "\n",
      "sqP = \n",
      "[[0.44761727451087113, 0.0, 0.0], [-0.05444303156447959, 0.3970089444102425, 0.0], [-0.02598152401458833, -0.057170444906944735, 0.2859559231817053]]\n",
      "sigma = \n",
      "[[-0.27729309  4.4097446   9.81672569]\n",
      " [ 0.61794146  4.4097446   9.81672569]\n",
      " [-0.38617915  5.20376249  9.81672569]\n",
      " [-0.32925614  4.29540371 10.38863753]\n",
      " [-1.17252764  4.4097446   9.81672569]\n",
      " [-0.16840703  3.61572671  9.81672569]\n",
      " [-0.22533004  4.52408549  9.24481384]]\n",
      "sqP = \n",
      "[[0.6356101853467989, 0.0, 0.0], [-0.023733841567539, 0.5996239984161459, 0.0], [0.015235554352886881, -0.02963886538905744, 0.5298032413877554]]\n",
      "m = \n",
      "[-0.27282998  4.48675695  9.69671196]\n",
      "P = \n",
      "[[ 0.20281622 -0.02582644 -0.01087501]\n",
      " [-0.02582644  0.17208423 -0.02218762]\n",
      " [-0.01087501 -0.02218762  0.08186904]]\n",
      "num = \n",
      "590\n",
      "SENSOR 9 1.14054428171 -0.998313577557\n",
      "\n",
      "num = \n",
      "591\n",
      "SENSOR 3 3.55052396622 -2.27149420334\n",
      "\n",
      "num = \n",
      "592\n",
      "SENSOR 6 4.97342840955 -0.481153301138\n",
      "\n",
      "num = \n",
      "593\n",
      "SENSOR 7 4.93140206369 -1.5097071492\n",
      "\n",
      "num = \n",
      "594\n",
      "ODOMETRY -0.000306172617703 0.0998659733612 0.00088045160042\n",
      "\n",
      "sqP = \n",
      "[[0.45035121459247546, 0.0, 0.0], [-0.05734733983821235, 0.41084731471572067, 0.0], [-0.024147833284785262, -0.05737516635717503, 0.2792740754316819]]\n",
      "sigma = \n",
      "[[-0.27282998  4.48675695  9.69671196]\n",
      " [ 0.62787245  4.48675695  9.69671196]\n",
      " [-0.38752466  5.30845158  9.69671196]\n",
      " [-0.32112564  4.37200662 10.25526011]\n",
      " [-1.17353241  4.48675695  9.69671196]\n",
      " [-0.1581353   3.66506232  9.69671196]\n",
      " [-0.22453431  4.60150728  9.13816381]]\n",
      "sqP = \n",
      "[[0.6377209826591775, 0.0, 0.0], [-0.02717840985335097, 0.608703198857883, 0.0], [0.01661662250656285, -0.02881895538243356, 0.5263702001481952]]\n",
      "m = \n",
      "[-0.18185577  4.42482507  9.73784594]\n",
      "P = \n",
      "[[ 0.17090609 -0.01427939 -0.01436419]\n",
      " [-0.01427939  0.09771733 -0.00489483]\n",
      " [-0.01436419 -0.00489483  0.07842931]]\n",
      "num = \n",
      "595\n",
      "SENSOR 9 0.908943386111 -1.05078434189\n",
      "\n",
      "num = \n",
      "596\n",
      "SENSOR 3 3.92854556221 -2.37317280336\n",
      "\n",
      "num = \n",
      "597\n",
      "SENSOR 6 4.65017824828 -0.48503026696\n",
      "\n",
      "num = \n",
      "598\n",
      "SENSOR 7 4.82256937785 -1.46085672931\n",
      "\n",
      "num = \n",
      "599\n",
      "ODOMETRY 6.35865396476e-05 0.100145996251 0.000825376841077\n",
      "\n",
      "sqP = \n",
      "[[0.41340790186919285, 0.0, 0.0], [-0.03454068548138969, 0.3106835468140331, 0.0], [-0.03474580664136655, -0.019617963718617765, 0.2771951850226018]]\n",
      "sigma = \n",
      "[[-0.18185577  4.42482507  9.73784594]\n",
      " [ 0.64496004  4.42482507  9.73784594]\n",
      " [-0.25093714  5.04619216  9.73784594]\n",
      " [-0.25134738  4.38558914 10.29223631]\n",
      " [-1.00867157  4.42482507  9.73784594]\n",
      " [-0.1127744   3.80345798  9.73784594]\n",
      " [-0.11236415  4.464061    9.18345557]]\n",
      "sqP = \n",
      "[[0.6109880713733875, 0.0, 0.0], [-0.011873731753817484, 0.5446434359365485, 0.0], [0.009069466382526451, -0.011530982087523662, 0.525420365780673]]\n",
      "m = \n",
      "[-0.19015068  4.70688825  9.73642886]\n",
      "P = \n",
      "[[ 0.1609692  -0.01218556 -0.01478258]\n",
      " [-0.01218556  0.09035201  0.00042305]\n",
      " [-0.01478258  0.00042305  0.07498196]]\n",
      "num = \n",
      "600\n",
      "SENSOR 9 0.827243169323 -1.09325181967\n",
      "\n",
      "num = \n",
      "601\n",
      "SENSOR 3 3.73413074447 -2.37141305415\n",
      "\n",
      "num = \n",
      "602\n",
      "SENSOR 6 4.78905396728 -0.408920738147\n",
      "\n",
      "num = \n",
      "603\n",
      "SENSOR 7 4.99209306977 -1.55682191086\n",
      "\n",
      "num = \n",
      "604\n",
      "ODOMETRY 0.000201838205791 0.100136187552 -0.00014387330981\n",
      "\n",
      "sqP = \n",
      "[[0.4012096709089756, 0.0, 0.0], [-0.03037205279072562, 0.2990477424616114, 0.0], [-0.036845022505246544, -0.0023274174368148512, 0.27132819373634803]]\n",
      "sigma = \n",
      "[[-0.19015068  4.70688825  9.73642886]\n",
      " [ 0.61226866  4.70688825  9.73642886]\n",
      " [-0.25089479  5.30498374  9.73642886]\n",
      " [-0.26384073  4.70223342 10.27908525]\n",
      " [-0.99257003  4.70688825  9.73642886]\n",
      " [-0.12940658  4.10879277  9.73642886]\n",
      " [-0.11646064  4.71154309  9.19377248]]\n",
      "sqP = \n",
      "[[0.6027015988903822, 0.0, 0.0], [-0.010327606599271425, 0.5377752173294357, 0.0], [0.007320537077504086, -0.0026340131733276003, 0.522371717789661]]\n",
      "m = \n",
      "[-0.18340839  4.67143552  9.76364108]\n",
      "P = \n",
      "[[ 0.16278405 -0.01138724 -0.01565743]\n",
      " [-0.01138724  0.09268931 -0.00468024]\n",
      " [-0.01565743 -0.00468024  0.07211178]]\n",
      "num = \n",
      "605\n",
      "SENSOR 9 0.854965120184 -1.30243719417\n",
      "\n",
      "num = \n",
      "606\n",
      "SENSOR 3 4.08986785793 -2.31375803278\n",
      "\n",
      "num = \n",
      "607\n",
      "SENSOR 6 4.49746631975 -0.415197960969\n",
      "\n",
      "num = \n",
      "608\n",
      "SENSOR 7 4.91878504334 -1.53841931157\n",
      "\n",
      "num = \n",
      "609\n",
      "ODOMETRY 0.000309566514052 0.099997143531 0.000424327411615\n",
      "\n",
      "sqP = \n",
      "[[0.4034650531512743, 0.0, 0.0], [-0.02822360807111282, 0.3031381575492824, 0.0], [-0.03880740759083769, -0.01905243920688852, 0.26503352438725986]]\n",
      "sigma = \n",
      "[[-0.18340839  4.67143552  9.76364108]\n",
      " [ 0.62352172  4.67143552  9.76364108]\n",
      " [-0.23985561  5.27771184  9.76364108]\n",
      " [-0.2610232   4.63333065 10.29370813]\n",
      " [-0.9903385   4.67143552  9.76364108]\n",
      " [-0.12696117  4.06515921  9.76364108]\n",
      " [-0.10579357  4.7095404   9.23357403]]\n",
      "sqP = \n",
      "[[0.6042239949313779, 0.0, 0.0], [-0.008467985062076208, 0.5404758887242467, 0.0], [0.007092430901561931, -0.010619366671494114, 0.5189879362072662]]\n",
      "m = \n",
      "[-0.185622    4.90919919  9.79551963]\n",
      "P = \n",
      "[[ 0.16271045 -0.01123811 -0.01529013]\n",
      " [-0.01123811  0.0929799  -0.00405646]\n",
      " [-0.01529013 -0.00405646  0.07128571]]\n",
      "num = \n",
      "610\n",
      "SENSOR 9 0.829574081247 -1.34777510094\n",
      "\n",
      "num = \n",
      "611\n",
      "SENSOR 3 3.99649915462 -2.37661430213\n",
      "\n",
      "num = \n",
      "612\n",
      "SENSOR 6 4.86215564276 -0.354259931521\n",
      "\n",
      "num = \n",
      "613\n",
      "SENSOR 7 5.17293988297 -1.61785902826\n",
      "\n",
      "num = \n",
      "614\n",
      "ODOMETRY -0.000499775865667 0.0998879891969 -0.000202950445095\n",
      "\n",
      "sqP = \n",
      "[[0.4033738314340973, 0.0, 0.0], [-0.02786029472992003, 0.3036506206439442, 0.0], [-0.037905601729760596, -0.016836846487322154, 0.26375252675824107]]\n",
      "sigma = \n",
      "[[-0.185622    4.90919919  9.79551963]\n",
      " [ 0.62112566  4.90919919  9.79551963]\n",
      " [-0.24134259  5.51650044  9.79551963]\n",
      " [-0.2614332   4.8755255  10.32302469]\n",
      " [-0.99236966  4.90919919  9.79551963]\n",
      " [-0.12990141  4.30189795  9.79551963]\n",
      " [-0.10981079  4.94287289  9.26801458]]\n",
      "sqP = \n",
      "[[0.6040889657581546, 0.0, 0.0], [-0.00843866468166003, 0.5406877432135201, 0.0], [0.00750305890396542, -0.009472677520661095, 0.5184036733454979]]\n",
      "m = \n",
      "[-0.18836141  4.77894137  9.93458518]\n",
      "P = \n",
      "[[ 0.16804749 -0.00958634 -0.015027  ]\n",
      " [-0.00958634  0.09550449 -0.00867053]\n",
      " [-0.015027   -0.00867053  0.06753909]]\n",
      "num = \n",
      "615\n",
      "SENSOR 9 0.926234276526 -1.50863813963\n",
      "\n",
      "num = \n",
      "616\n",
      "SENSOR 3 4.06561199195 -2.33294732347\n",
      "\n",
      "num = \n",
      "617\n",
      "SENSOR 6 4.52518969077 -0.459458859882\n",
      "\n",
      "num = \n",
      "618\n",
      "SENSOR 7 4.90713478296 -1.6534950895\n",
      "\n",
      "num = \n",
      "619\n",
      "ODOMETRY 0.000159821856273 0.0999413115697 0.000165823443296\n",
      "\n",
      "sqP = \n",
      "[[0.4099359541162769, 0.0, 0.0], [-0.02338496204412358, 0.3081519681852053, 0.0], [-0.03665693830549797, -0.0309190114647313, 0.2554199929407581]]\n",
      "sigma = \n",
      "[[-0.18836141  4.77894137  9.93458518]\n",
      " [ 0.6315105   4.77894137  9.93458518]\n",
      " [-0.23513133  5.39524531  9.93458518]\n",
      " [-0.26167528  4.71710335 10.44542516]\n",
      " [-1.00823331  4.77894137  9.93458518]\n",
      " [-0.14159148  4.16263744  9.93458518]\n",
      " [-0.11504753  4.8407794   9.42374519]]\n",
      "sqP = \n",
      "[[0.6082253480848694, 0.0, 0.0], [-0.005306058127092324, 0.5439541667039154, 0.0], [0.00908193979996835, -0.015452508940811854, 0.5141920278733977]]\n",
      "m = \n",
      "[-0.12859191  4.93299065  9.86048617]\n",
      "P = \n",
      "[[ 0.16476793 -0.01104433 -0.01261077]\n",
      " [-0.01104433  0.09694989 -0.00444432]\n",
      " [-0.01261077 -0.00444432  0.06504895]]\n",
      "num = \n",
      "620\n",
      "SENSOR 9 0.798977474978 -1.61450594503\n",
      "\n",
      "num = \n",
      "621\n",
      "SENSOR 3 4.24005259522 -2.48339290405\n",
      "\n",
      "num = \n",
      "622\n",
      "SENSOR 6 4.37753199658 -0.439993648344\n",
      "\n",
      "num = \n",
      "623\n",
      "SENSOR 7 4.78729857286 -1.52285334952\n",
      "\n",
      "num = \n",
      "624\n",
      "ODOMETRY 0.000239336090803 0.100024339541 -0.000702745140684\n",
      "\n",
      "sqP = \n",
      "[[0.40591616486441573, 0.0, 0.0], [-0.02720838960872706, 0.3101767092366032, 0.0], [-0.031067426802017257, -0.01705356308019402, 0.2525726417228574]]\n",
      "sigma = \n",
      "[[-0.12859191  4.93299065  9.86048617]\n",
      " [ 0.68324042  4.93299065  9.86048617]\n",
      " [-0.18300869  5.55334407  9.86048617]\n",
      " [-0.19072676  4.89888352 10.36563146]\n",
      " [-0.94042424  4.93299065  9.86048617]\n",
      " [-0.07417513  4.31263723  9.86048617]\n",
      " [-0.06645706  4.96709777  9.35534089]]\n",
      "sqP = \n",
      "[[0.6053704769572387, 0.0, 0.0], [-0.009915979415907947, 0.5444257139123477, 0.0], [0.011427585048861447, -0.009058645205894493, 0.5131651021397826]]\n",
      "m = \n",
      "[-0.13910213  5.10742999  9.8073325 ]\n",
      "P = \n",
      "[[ 0.16910339 -0.00962112 -0.01328132]\n",
      " [-0.00962112  0.09743032 -0.00845674]\n",
      " [-0.01328132 -0.00845674  0.06450588]]\n",
      "num = \n",
      "625\n",
      "SENSOR 9 0.85153318303 -1.74081822727\n",
      "\n",
      "num = \n",
      "626\n",
      "SENSOR 3 4.19445086514 -2.41078939754\n",
      "\n",
      "num = \n",
      "627\n",
      "SENSOR 6 4.3332692815 -0.5738717181\n",
      "\n",
      "num = \n",
      "628\n",
      "SENSOR 7 4.82886107215 -1.58958533264\n",
      "\n",
      "num = \n",
      "629\n",
      "ODOMETRY 0.000302502313308 0.0999098702903 -0.000677343238\n",
      "\n",
      "sqP = \n",
      "[[0.411221824189465, 0.0, 0.0], [-0.023396418497449646, 0.31126023140632497, 0.0], [-0.03229721719011701, -0.029597017929392906, 0.25017350878113137]]\n",
      "sigma = \n",
      "[[-0.13910213  5.10742999  9.8073325 ]\n",
      " [ 0.68334152  5.10742999  9.8073325 ]\n",
      " [-0.18589497  5.72995046  9.8073325 ]\n",
      " [-0.20369656  5.04823596 10.30767951]\n",
      " [-0.96154578  5.10742999  9.8073325 ]\n",
      " [-0.09230929  4.48490953  9.8073325 ]\n",
      " [-0.07450769  5.16662403  9.30698548]]\n",
      "sqP = \n",
      "[[0.6088463610193007, 0.0, 0.0], [-0.006928473695833437, 0.5456633481957845, 0.0], [0.011502968636113506, -0.014482874440481432, 0.511841473453829]]\n",
      "m = \n",
      "[-0.11513546  5.1543982   9.8090251 ]\n",
      "P = \n",
      "[[ 0.17327919 -0.0079787  -0.01352262]\n",
      " [-0.0079787   0.09666863 -0.01269525]\n",
      " [-0.01352262 -0.01269525  0.06544673]]\n",
      "num = \n",
      "630\n",
      "SENSOR 9 1.00720101453 -1.80629458243\n",
      "\n",
      "num = \n",
      "631\n",
      "SENSOR 3 4.35581118198 -2.52872962107\n",
      "\n",
      "num = \n",
      "632\n",
      "SENSOR 6 4.26273136747 -0.561343932255\n",
      "\n",
      "num = \n",
      "633\n",
      "SENSOR 7 4.90576859962 -1.57561915161\n",
      "\n",
      "num = \n",
      "634\n",
      "ODOMETRY -0.000113543700968 0.0999631337801 5.71158197463e-05\n",
      "\n",
      "sqP = \n",
      "[[0.416268174554532, 0.0, 0.0], [-0.019167211733901793, 0.3103244245871777, 0.0], [-0.032485365551942966, -0.04291607511253134, 0.2500992627074666]]\n",
      "sigma = \n",
      "[[-0.11513546  5.1543982   9.8090251 ]\n",
      " [ 0.71740089  5.1543982   9.8090251 ]\n",
      " [-0.15346988  5.77504705  9.8090251 ]\n",
      " [-0.18010619  5.06856605 10.30922362]\n",
      " [-0.94767181  5.1543982   9.8090251 ]\n",
      " [-0.07680104  4.53374935  9.8090251 ]\n",
      " [-0.05016473  5.24023035  9.30882657]]\n",
      "sqP = \n",
      "[[0.6121289685414815, 0.0, 0.0], [-0.0045338737394276475, 0.5461068588894804, 0.0], [0.011918774828743395, -0.020306937089269957, 0.5116126308869285]]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "m = \n",
      "[-0.12483323  5.25235163  9.87774225]\n",
      "P = \n",
      "[[ 0.17509236 -0.00734003 -0.01336523]\n",
      " [-0.00734003  0.09642473 -0.0138953 ]\n",
      " [-0.01336523 -0.0138953   0.06555608]]\n",
      "num = \n",
      "635\n",
      "SENSOR 9 0.926859129886 -2.02052805263\n",
      "\n",
      "num = \n",
      "636\n",
      "SENSOR 3 4.5099200538 -2.5031268995\n",
      "\n",
      "num = \n",
      "637\n",
      "SENSOR 6 4.1118649023 -0.581633953961\n",
      "\n",
      "num = \n",
      "638\n",
      "SENSOR 7 4.76898001883 -1.59231791735\n",
      "\n",
      "num = \n",
      "639\n",
      "ODOMETRY -0.000339024488948 0.100009711461 2.26344713328e-05\n",
      "\n",
      "sqP = \n",
      "[[0.4184403872589866, 0.0, 0.0], [-0.017541402438594783, 0.31002745743912064, 0.0], [-0.03194058020720259, -0.0466267954608574, 0.24972349400647878]]\n",
      "sigma = \n",
      "[[-0.12483323  5.25235163  9.87774225]\n",
      " [ 0.71204755  5.25235163  9.87774225]\n",
      " [-0.15991603  5.87240654  9.87774225]\n",
      " [-0.18871439  5.15909804 10.37718924]\n",
      " [-0.961714    5.25235163  9.87774225]\n",
      " [-0.08975042  4.63229671  9.87774225]\n",
      " [-0.06095207  5.34560522  9.37829526]]\n",
      "sqP = \n",
      "[[0.6135309765227313, 0.0, 0.0], [-0.003255106409500138, 0.5462634521242151, 0.0], [0.012329167019957624, -0.02187187635410692, 0.5113927489032005]]\n",
      "m = \n",
      "[-0.11706902  5.40327022  9.85517924]\n",
      "P = \n",
      "[[ 0.17658863 -0.00665208 -0.01246977]\n",
      " [-0.00665208  0.09700108 -0.0149412 ]\n",
      " [-0.01246977 -0.0149412   0.06408027]]\n",
      "num = \n",
      "640\n",
      "SENSOR 9 1.01426369829 -2.06024600625\n",
      "\n",
      "num = \n",
      "641\n",
      "SENSOR 3 4.50108503541 -2.50066382553\n",
      "\n",
      "num = \n",
      "642\n",
      "SENSOR 6 4.10077035421 -0.552157899173\n",
      "\n",
      "num = \n",
      "643\n",
      "SENSOR 7 4.90578246686 -1.60737195764\n",
      "\n",
      "num = \n",
      "644\n",
      "ODOMETRY -0.000504457264396 0.100013657645 -0.000177576169986\n",
      "\n",
      "sqP = \n",
      "[[0.42022450423431185, 0.0, 0.0], [-0.015829831734042073, 0.31104741666690927, 0.0], [-0.029674059886001393, -0.049545290951353176, 0.24646497995694966]]\n",
      "sigma = \n",
      "[[-0.11706902  5.40327022  9.85517924]\n",
      " [ 0.72337998  5.40327022  9.85517924]\n",
      " [-0.14872869  6.02536506  9.85517924]\n",
      " [-0.17641714  5.30417964 10.3481092 ]\n",
      " [-0.95751803  5.40327022  9.85517924]\n",
      " [-0.08540936  4.78117539  9.85517924]\n",
      " [-0.0577209   5.50236081  9.36224928]]\n",
      "sqP = \n",
      "[[0.6145891044932535, 0.0, 0.0], [-0.002614307901394455, 0.5471203662202188, 0.0], [0.013577505358547645, -0.022784124812994164, 0.5098746354993607]]\n",
      "m = \n",
      "[-0.2456752   5.43834543  9.8987995 ]\n",
      "P = \n",
      "[[ 0.17101618 -0.00303346  0.00600935]\n",
      " [-0.00303346  0.09504521 -0.01657408]\n",
      " [ 0.00600935 -0.01657408  0.06613542]]\n",
      "num = \n",
      "645\n",
      "SENSOR 9 0.887211360034 -2.18963300619\n",
      "\n",
      "num = \n",
      "646\n",
      "SENSOR 3 4.5749440351 -2.48258346425\n",
      "\n",
      "num = \n",
      "647\n",
      "SENSOR 6 3.90635999301 -0.524342529557\n",
      "\n",
      "num = \n",
      "648\n",
      "SENSOR 7 4.99010419492 -1.6897455089\n",
      "\n",
      "num = \n",
      "649\n",
      "ODOMETRY 0.000243302162545 0.100124792442 0.000564451310213\n",
      "\n",
      "sqP = \n",
      "[[0.4135410232881161, 0.0, 0.0], [-0.007335337013106157, 0.3082067536804327, 0.0], [0.014531436958438396, -0.05342998820213932, 0.2511364012324419]]\n",
      "sigma = \n",
      "[[-0.2456752   5.43834543  9.8987995 ]\n",
      " [ 0.58140685  5.43834543  9.8987995 ]\n",
      " [-0.26034587  6.05475894  9.8987995 ]\n",
      " [-0.21661233  5.33148545 10.4010723 ]\n",
      " [-1.07275725  5.43834543  9.8987995 ]\n",
      " [-0.23100453  4.82193192  9.8987995 ]\n",
      " [-0.27473807  5.5452054   9.3965267 ]]\n",
      "sqP = \n",
      "[[0.6093284399823753, 0.0, 0.0], [0.0011131160706629945, 0.5458583147735454, 0.0], [0.030255849914184132, -0.02451730864089584, 0.5133701975527902]]\n",
      "m = \n",
      "[-0.24257773  5.54595867  9.79359596]\n",
      "P = \n",
      "[[ 0.15382659 -0.01487748  0.00761265]\n",
      " [-0.01487748  0.09050944 -0.01113864]\n",
      " [ 0.00761265 -0.01113864  0.07462116]]\n",
      "num = \n",
      "650\n",
      "SENSOR 9 0.950336814138 -2.20135217535\n",
      "\n",
      "num = \n",
      "651\n",
      "SENSOR 3 4.64575251122 -2.45474381848\n",
      "\n",
      "num = \n",
      "652\n",
      "SENSOR 6 3.94194574093 -0.493874197364\n",
      "\n",
      "num = \n",
      "653\n",
      "SENSOR 7 5.01810231369 -1.75192836429\n",
      "\n",
      "num = \n",
      "654\n",
      "ODOMETRY -0.000738312989045 0.100017614324 0.000452576665471\n",
      "\n",
      "sqP = \n",
      "[[0.3922073306619064, 0.0, 0.0], [-0.03793268679174304, 0.29844690125307244, 0.0], [0.01940974713052626, -0.034855044815860686, 0.270239805060462]]\n",
      "sigma = \n",
      "[[-0.24257773  5.54595867  9.79359596]\n",
      " [ 0.54183693  5.54595867  9.79359596]\n",
      " [-0.3184431   6.14285247  9.79359596]\n",
      " [-0.20375824  5.47624858 10.33407557]\n",
      " [-1.02699239  5.54595867  9.79359596]\n",
      " [-0.16671236  4.94906487  9.79359596]\n",
      " [-0.28139722  5.61566876  9.25311635]]\n",
      "sqP = \n",
      "[[0.5963574576005983, 0.0, 0.0], [-0.014463458342106236, 0.5382595241736615, 0.0], [0.03166993728958128, -0.018098577993288786, 0.5233768783244876]]\n",
      "m = \n",
      "[-0.32426     5.58211082  9.89811174]\n",
      "P = \n",
      "[[ 0.16257037 -0.00580372  0.00995289]\n",
      " [-0.00580372  0.0902997  -0.01826745]\n",
      " [ 0.00995289 -0.01826745  0.07037335]]\n",
      "num = \n",
      "655\n",
      "SENSOR 9 1.24742332493 -2.3761257833\n",
      "\n",
      "num = \n",
      "656\n",
      "SENSOR 3 4.78533958114 -2.48110189333\n",
      "\n",
      "num = \n",
      "657\n",
      "SENSOR 6 3.83981394901 -0.473736803464\n",
      "\n",
      "num = \n",
      "658\n",
      "SENSOR 7 4.94028734088 -1.67119507241\n",
      "\n",
      "num = \n",
      "659\n",
      "ODOMETRY -0.000158281123923 0.100057519587 8.25146303648e-05\n",
      "\n",
      "sqP = \n",
      "[[0.4032001600635974, 0.0, 0.0], [-0.014394152746116108, 0.30015413521558587, 0.0], [0.02468473852860177, -0.059676442965039635, 0.2572989294975788]]\n",
      "sigma = \n",
      "[[-0.32426     5.58211082  9.89811174]\n",
      " [ 0.48214032  5.58211082  9.89811174]\n",
      " [-0.35304831  6.18241909  9.89811174]\n",
      " [-0.27489052  5.46275793 10.4127096 ]\n",
      " [-1.13066032  5.58211082  9.89811174]\n",
      " [-0.29547169  4.98180255  9.89811174]\n",
      " [-0.37362948  5.7014637   9.38351388]]\n",
      "sqP = \n",
      "[[0.6028158068803559, 0.0, 0.0], [-0.0018584042889578636, 0.5418239160850765, 0.0], [0.033544082075299554, -0.028229623667072642, 0.5164103207845283]]\n",
      "m = \n",
      "[-0.28293073  5.73549617  9.84308812]\n",
      "P = \n",
      "[[ 0.15198116 -0.01461718  0.00807228]\n",
      " [-0.01461718  0.08772407 -0.01357626]\n",
      " [ 0.00807228 -0.01357626  0.0776816 ]]\n",
      "num = \n",
      "660\n",
      "SENSOR 9 1.25516136996 -2.38310746911\n",
      "\n",
      "num = \n",
      "661\n",
      "SENSOR 3 4.67629555186 -2.57984253808\n",
      "\n",
      "num = \n",
      "662\n",
      "SENSOR 6 3.58479173705 -0.552269803889\n",
      "\n",
      "num = \n",
      "663\n",
      "SENSOR 7 5.04082293108 -1.86475241589\n",
      "\n",
      "num = \n",
      "664\n",
      "ODOMETRY -0.000421693045302 0.100088421092 -0.000583909844015\n",
      "\n",
      "sqP = \n",
      "[[0.38984761046533584, 0.0, 0.0], [-0.037494612014061866, 0.2937996396404688, 0.0], [0.02070624390017929, -0.043566729468484645, 0.2745082663061495]]\n",
      "sigma = \n",
      "[[-0.28293073  5.73549617  9.84308812]\n",
      " [ 0.49676449  5.73549617  9.84308812]\n",
      " [-0.35791995  6.32309545  9.84308812]\n",
      " [-0.24151824  5.64836271 10.39210465]\n",
      " [-1.06262595  5.73549617  9.84308812]\n",
      " [-0.2079415   5.14789689  9.84308812]\n",
      " [-0.32434322  5.82262963  9.29407159]]\n",
      "sqP = \n",
      "[[0.5948241369111301, 0.0, 0.0], [-0.013510928253492463, 0.5362922004258335, 0.0], [0.03198696355601942, -0.022781187546434246, 0.5254276170671982]]\n",
      "m = \n",
      "[-0.30035874  5.84570656  9.78259044]\n",
      "P = \n",
      "[[ 0.1663565  -0.00387741 -0.00449811]\n",
      " [-0.00387741  0.08528077 -0.01652391]\n",
      " [-0.00449811 -0.01652391  0.08284622]]\n",
      "num = \n",
      "665\n",
      "SENSOR 9 1.30043780911 -2.42814444723\n",
      "\n",
      "num = \n",
      "666\n",
      "SENSOR 3 4.77942694559 -2.51108430416\n",
      "\n",
      "num = \n",
      "667\n",
      "SENSOR 6 3.7036104257 -0.594306761593\n",
      "\n",
      "num = \n",
      "668\n",
      "SENSOR 7 5.20342066846 -1.82597751664\n",
      "\n",
      "num = \n",
      "669\n",
      "ODOMETRY -0.000492703155817 0.0998767019988 -0.000399546911055\n",
      "\n",
      "sqP = \n",
      "[[0.4078682425600564, 0.0, 0.0], [-0.009506516678126005, 0.29187394176568543, 0.0], [-0.011028338292839298, -0.056972388214844155, 0.2819197393257387]]\n",
      "sigma = \n",
      "[[-0.30035874  5.84570656  9.78259044]\n",
      " [ 0.51537775  5.84570656  9.78259044]\n",
      " [-0.31937177  6.42945445  9.78259044]\n",
      " [-0.32241541  5.73176179 10.34642992]\n",
      " [-1.11609522  5.84570656  9.78259044]\n",
      " [-0.2813457   5.26195868  9.78259044]\n",
      " [-0.27830206  5.95965134  9.21875097]]\n",
      "sqP = \n",
      "[[0.6054490080872887, 0.0, 0.0], [0.0037388045785337882, 0.5371975936819656, 0.0], [0.018346835956608538, -0.029977341439215748, 0.5280898844422491]]\n",
      "m = \n",
      "[-0.31296672  5.87351779  9.87629397]\n",
      "P = \n",
      "[[ 0.17185304 -0.00106245 -0.00727205]\n",
      " [-0.00106245  0.08128983 -0.01700247]\n",
      " [-0.00727205 -0.01700247  0.08594596]]\n",
      "num = \n",
      "670\n",
      "SENSOR 9 1.50701523022 -2.40519251821\n",
      "\n",
      "num = \n",
      "671\n",
      "SENSOR 3 4.97415244053 -2.57426809268\n",
      "\n",
      "num = \n",
      "672\n",
      "SENSOR 6 3.73937872599 -0.646973765524\n",
      "\n",
      "num = \n",
      "673\n",
      "SENSOR 7 5.04630156868 -1.81126053621\n",
      "\n",
      "num = \n",
      "674\n",
      "ODOMETRY -0.000686941708413 0.100091264802 -0.000646733220067\n",
      "\n",
      "sqP = \n",
      "[[0.4145516129695687, 0.0, 0.0], [-0.0025629015172253565, 0.28510219561039346, 0.0], [-0.0175419584994902, -0.05979409052738833, 0.2864662366356693]]\n",
      "sigma = \n",
      "[[-0.31296672  5.87351779  9.87629397]\n",
      " [ 0.5161365   5.87351779  9.87629397]\n",
      " [-0.31809252  6.44372218  9.87629397]\n",
      " [-0.34805064  5.75392961 10.44922644]\n",
      " [-1.14206995  5.87351779  9.87629397]\n",
      " [-0.30784092  5.3033134   9.87629397]\n",
      " [-0.2778828   5.99310597  9.3033615 ]]\n",
      "sqP = \n",
      "[[0.6100551848708902, 0.0, 0.0], [0.008252714082234748, 0.5339637025459552, 0.0], [0.015613392177311275, -0.03191497214984419, 0.5301832448564346]]\n",
      "m = \n",
      "[-0.31262889  5.95815086  9.91332954]\n",
      "P = \n",
      "[[ 0.17409599 -0.00024757 -0.00744835]\n",
      " [-0.00024757  0.08278327 -0.0174345 ]\n",
      " [-0.00744835 -0.0174345   0.08405685]]\n",
      "num = \n",
      "675\n",
      "SENSOR 9 1.53949734641 -2.56047560031\n",
      "\n",
      "num = \n",
      "676\n",
      "SENSOR 6 3.54771183278 -0.554710714875\n",
      "\n",
      "num = \n",
      "677\n",
      "ODOMETRY 0.000264360795126 0.0999909746645 -0.000504895193083\n",
      "\n",
      "sqP = \n",
      "[[0.41724811517638777, 0.0, 0.0], [-0.0005933311191226365, 0.2877202121886362, 0.0], [-0.017851121260556328, -0.06063212833413804, 0.28295217833863795]]\n",
      "sigma = \n",
      "[[-0.31262889  5.95815086  9.91332954]\n",
      " [ 0.52186734  5.95815086  9.91332954]\n",
      " [-0.31381556  6.53359129  9.91332954]\n",
      " [-0.34833114  5.83688661 10.4792339 ]\n",
      " [-1.14712512  5.95815086  9.91332954]\n",
      " [-0.31144223  5.38271044  9.91332954]\n",
      " [-0.27692665  6.07941512  9.34742518]]\n",
      "sqP = \n",
      "[[0.6118946021908709, 0.0, 0.0], [0.00929941585436901, 0.5354839055090864, 0.0], [0.01584978290161896, -0.03179855784039424, 0.5283033424150643]]\n",
      "m = \n",
      "[-0.39411941  6.08340606  9.87057418]\n",
      "P = \n",
      "[[ 0.22284308  0.00346629 -0.01953002]\n",
      " [ 0.00346629  0.10874193  0.00591768]\n",
      " [-0.01953002  0.00591768  0.15716638]]\n",
      "num = \n",
      "678\n",
      "SENSOR 9 1.51238925612 -2.52278575906\n",
      "\n",
      "num = \n",
      "679\n",
      "SENSOR 6 3.27990661032 -0.58279346945\n",
      "\n",
      "num = \n",
      "680\n",
      "ODOMETRY -1.55641718837e-05 0.100163521473 -0.000133745408038\n",
      "\n",
      "sqP = \n",
      "[[0.47206257736322726, 0.0, 0.0], [0.007342858028611868, 0.329678648019186, 0.0], [-0.04137166765541563, 0.018871317104902155, 0.3938256477510105]]\n",
      "sigma = \n",
      "[[-0.39411941  6.08340606  9.87057418]\n",
      " [ 0.55000574  6.08340606  9.87057418]\n",
      " [-0.37943369  6.74276335  9.87057418]\n",
      " [-0.47686275  6.12114869 10.65822548]\n",
      " [-1.33824457  6.08340606  9.87057418]\n",
      " [-0.40880513  5.42404876  9.87057418]\n",
      " [-0.31137608  6.04566342  9.08292289]]\n",
      "sqP = \n",
      "[[0.6516199884867188, 0.0, 0.0], [0.01387875056744134, 0.5563213022854675, 0.0], [0.0023363802915297525, 0.013303463218735108, 0.5944502603408404]]\n",
      "m = \n",
      "[-0.426829    6.22932889  9.70346969]\n",
      "P = \n",
      "[[ 0.25191103  0.00098363 -0.03255553]\n",
      " [ 0.00098363  0.10976736  0.01438445]\n",
      " [-0.03255553  0.01438445  0.19031292]]\n",
      "num = \n",
      "681\n",
      "SENSOR 9 1.5098952529 -2.57185137154\n",
      "\n",
      "num = \n",
      "682\n",
      "SENSOR 6 3.39256354505 -0.708652206512\n",
      "\n",
      "num = \n",
      "683\n",
      "ODOMETRY -0.00944281048405 0.0998363016919 -0.0197064530302\n",
      "\n",
      "sqP = \n",
      "[[0.5019073924483379, 0.0, 0.0], [0.00195979328772572, 0.3313057754494237, 0.0], [-0.06486362348419827, 0.043801148767936705, 0.4291702298585577]]\n",
      "sigma = \n",
      "[[-0.426829    6.22932889  9.70346969]\n",
      " [ 0.57698578  6.22932889  9.70346969]\n",
      " [-0.42290941  6.89194044  9.70346969]\n",
      " [-0.55655625  6.31693118 10.56181015]\n",
      " [-1.43064379  6.22932889  9.70346969]\n",
      " [-0.43074859  5.56671733  9.70346969]\n",
      " [-0.29710175  6.14172659  8.84512923]]\n",
      "sqP = \n",
      "[[0.6753681670285626, 0.0, 0.0], [0.0100100485387592, 0.5586088978505407, 0.0], [-0.012073217626814117, 0.03241450693754565, 0.6160442003666585]]\n",
      "m = \n",
      "[-0.45061933  6.26709923  9.95757766]\n",
      "P = \n",
      "[[0.26802222 0.00651835 0.02846248]\n",
      " [0.00651835 0.09859554 0.00607574]\n",
      " [0.02846248 0.00607574 0.17455798]]\n",
      "num = \n",
      "684\n",
      "SENSOR 9 1.7405212542 -2.61856151363\n",
      "\n",
      "num = \n",
      "685\n",
      "SENSOR 6 3.4449354883 -0.653656313882\n",
      "\n",
      "num = \n",
      "686\n",
      "ODOMETRY -0.00982171572374 0.0999283940399 -0.0201816524328\n",
      "\n",
      "sqP = \n",
      "[[0.5177086217601783, 0.0, 0.0], [0.012590776239072154, 0.3137467402840149, 0.0], [0.05497779658164645, 0.017158833483224403, 0.41381275523132605]]\n",
      "sigma = \n",
      "[[-0.45061933  6.26709923  9.95757766]\n",
      " [ 0.58479792  6.26709923  9.95757766]\n",
      " [-0.42543777  6.89459271  9.95757766]\n",
      " [-0.34066373  6.30141689 10.78520317]\n",
      " [-1.48603657  6.26709923  9.95757766]\n",
      " [-0.47580088  5.63960575  9.95757766]\n",
      " [-0.56057492  6.23278156  9.12995215]]\n",
      "sqP = \n",
      "[[0.6864424978161672, 0.0, 0.0], [0.021710879676644982, 0.547193093950064, 0.0], [0.06256963614259355, 0.014172552886306377, 0.6105572932358668]]\n",
      "m = \n",
      "[-0.50814249  6.32410401  9.94223224]\n",
      "P = \n",
      "[[ 0.26691791  0.00253241 -0.01155114]\n",
      " [ 0.00253241  0.10772115  0.01338558]\n",
      " [-0.01155114  0.01338558  0.19313639]]\n",
      "num = \n",
      "687\n",
      "SENSOR 9 1.65301995925 -2.56952419519\n",
      "\n",
      "num = \n",
      "688\n",
      "SENSOR 6 3.02584430907 -0.625348664803\n",
      "\n",
      "num = \n",
      "689\n",
      "ODOMETRY -0.011467407889 0.100278553593 -0.0200899025046\n",
      "\n",
      "sqP = \n",
      "[[0.516640985817428, 0.0, 0.0], [0.004901683563773761, 0.3281724032554838, 0.0], [-0.022358153944584982, 0.04112221085288937, 0.43697306765151067]]\n",
      "sigma = \n",
      "[[-0.50814249  6.32410401  9.94223224]\n",
      " [ 0.52513948  6.32410401  9.94223224]\n",
      " [-0.49833912  6.98044881  9.94223224]\n",
      " [-0.5528588   6.40634843 10.81617837]\n",
      " [-1.54142446  6.32410401  9.94223224]\n",
      " [-0.51794586  5.6677592   9.94223224]\n",
      " [-0.46342618  6.24185958  9.0682861 ]]\n",
      "sqP = \n",
      "[[0.6836971711042638, 0.0, 0.0], [0.017103233824146656, 0.5567340312895943, 0.0], [0.013954243030910165, 0.032170424139512675, 0.6241181411985189]]\n",
      "m = \n",
      "[-0.54030825  6.46900707  9.68042023]\n",
      "P = \n",
      "[[ 0.27236847 -0.00049166 -0.03006001]\n",
      " [-0.00049166  0.10729734  0.01466189]\n",
      " [-0.03006001  0.01466189  0.19845133]]\n",
      "num = \n",
      "690\n",
      "SENSOR 9 1.84416645507 -2.56385985305\n",
      "\n",
      "num = \n",
      "691\n",
      "SENSOR 6 3.02960198478 -0.517486552302\n",
      "\n",
      "num = \n",
      "692\n",
      "ODOMETRY -0.00949998918607 0.100016536484 -0.0192452653175\n",
      "\n",
      "sqP = \n",
      "[[0.5218893314537417, 0.0, 0.0], [-0.0009420841399312462, 0.32756137724484885, 0.0], [-0.05759843796213952, 0.04459508616774804, 0.4394826798923167]]\n",
      "sigma = \n",
      "[[-0.54030825  6.46900707  9.68042023]\n",
      " [ 0.50347041  6.46900707  9.68042023]\n",
      " [-0.54219242  7.12412982  9.68042023]\n",
      " [-0.65550513  6.55819724 10.55938559]\n",
      " [-1.58408691  6.46900707  9.68042023]\n",
      " [-0.53842408  5.81388432  9.68042023]\n",
      " [-0.42511137  6.3798169   8.80145487]]\n",
      "sqP = \n",
      "[[0.689700617560575, 0.0, 0.0], [0.013270190700654607, 0.5564775220367789, 0.0], [-0.008366830545313626, 0.033755084072669306, 0.6238066350765306]]\n",
      "m = \n",
      "[-0.57771803  6.58212082  9.62580716]\n",
      "P = \n",
      "[[ 0.27987007 -0.00441187 -0.04487571]\n",
      " [-0.00441187  0.09788397  0.02201373]\n",
      " [-0.04487571  0.02201373  0.23322959]]\n",
      "num = \n",
      "693\n",
      "SENSOR 9 1.83946050295 -2.63003003289\n",
      "\n",
      "num = \n",
      "694\n",
      "SENSOR 6 3.00582309666 -0.577468671279\n",
      "\n",
      "num = \n",
      "695\n",
      "ODOMETRY -0.00998770181841 0.100041220527 -0.0210200884984\n",
      "\n",
      "sqP = \n",
      "[[0.5290274738762861, 0.0, 0.0], [-0.008339584927508014, 0.31275297679287756, 0.0], [-0.08482679657834696, 0.06812505458313127, 0.47052415369354217]]\n",
      "sigma = \n",
      "[[-0.57771803  6.58212082  9.62580716]\n",
      " [ 0.48033692  6.58212082  9.62580716]\n",
      " [-0.5943972   7.20762677  9.62580716]\n",
      " [-0.74737163  6.71837092 10.56685546]\n",
      " [-1.63577298  6.58212082  9.62580716]\n",
      " [-0.56103886  5.95661486  9.62580716]\n",
      " [-0.40806444  6.44587071  8.68475885]]\n",
      "sqP = \n",
      "[[0.6979507161758858, 0.0, 0.0], [0.006857806660378445, 0.5499256453638939, 0.0], [-0.028826131790151647, 0.054632234815685674, 0.6422424277091453]]\n",
      "m = \n",
      "[-0.58937455  6.62297402  9.57419416]\n",
      "P = \n",
      "[[ 0.28876633 -0.0079098  -0.0569655 ]\n",
      " [-0.0079098   0.09436533  0.02605861]\n",
      " [-0.0569655   0.02605861  0.25414486]]\n",
      "num = \n",
      "696\n",
      "SENSOR 9 2.1231956515 -2.5712329806\n",
      "\n",
      "num = \n",
      "697\n",
      "SENSOR 6 2.72060090136 -0.611342981081\n",
      "\n",
      "num = \n",
      "698\n",
      "ODOMETRY -0.0104435233728 0.100064843828 -0.0209923840156\n",
      "\n",
      "sqP = \n",
      "[[0.5373698241199691, 0.0, 0.0], [-0.014719480268910054, 0.3068365519974004, 0.0], [-0.10600799639357042, 0.07984127830310787, 0.4863461092037701]]\n",
      "sigma = \n",
      "[[-0.58937455  6.62297402  9.57419416]\n",
      " [ 0.4853651   6.62297402  9.57419416]\n",
      " [-0.61881351  7.23664712  9.57419416]\n",
      " [-0.80139054  6.78265658 10.54688638]\n",
      " [-1.6641142   6.62297402  9.57419416]\n",
      " [-0.55993559  6.00930092  9.57419416]\n",
      " [-0.37735856  6.46329146  8.60150194]]\n",
      "sqP = \n",
      "[[0.7072628127319383, 0.0, 0.0], [0.0014095345429433804, 0.5477745989066028, 0.0], [-0.0439697584398635, 0.06509653477133669, 0.6507409091428229]]\n",
      "m = \n",
      "[-0.57480322  6.85447287  9.51573646]\n",
      "P = \n",
      "[[ 0.29744372 -0.01041486 -0.06662511]\n",
      " [-0.01041486  0.09236486  0.02920753]\n",
      " [-0.06662511  0.02920753  0.26975782]]\n",
      "num = \n",
      "699\n",
      "SENSOR 9 2.2533253491 -2.51358315181\n",
      "\n",
      "num = \n",
      "700\n",
      "SENSOR 6 2.76502567229 -0.610392844017\n",
      "\n",
      "num = \n",
      "701\n",
      "ODOMETRY -0.0101773588174 0.100165429114 -0.0197769867168\n",
      "\n",
      "sqP = \n",
      "[[0.5453840069811509, 0.0, 0.0], [-0.019096371353155443, 0.3033153210585838, 0.0], [-0.12216183995545477, 0.08860311539534074, 0.49697464445462985]]\n",
      "sigma = \n",
      "[[-0.57480322  6.85447287  9.51573646]\n",
      " [ 0.51596479  6.85447287  9.51573646]\n",
      " [-0.61299596  7.46110351  9.51573646]\n",
      " [-0.8191269   7.0316791  10.50968575]\n",
      " [-1.66557124  6.85447287  9.51573646]\n",
      " [-0.53661048  6.24784223  9.51573646]\n",
      " [-0.33047954  6.67726664  8.52178717]]\n",
      "sqP = \n",
      "[[0.7160530019565421, 0.0, 0.0], [-0.003364850013827845, 0.5468326704156937, 0.0], [-0.054847154056995806, 0.07293582360013147, 0.6559430510031308]]\n",
      "m = \n",
      "[-0.56902668  6.97792155  9.56857756]\n",
      "P = \n",
      "[[ 0.30546681 -0.01410916 -0.07371191]\n",
      " [-0.01410916  0.09184157  0.03353884]\n",
      " [-0.07371191  0.03353884  0.27846528]]\n",
      "num = \n",
      "702\n",
      "SENSOR 9 2.27395359652 -2.62138399446\n",
      "\n",
      "num = \n",
      "703\n",
      "SENSOR 6 2.81928646279 -0.499702125893\n",
      "\n",
      "num = \n",
      "704\n",
      "ODOMETRY -0.00963806206216 0.100194140395 -0.0202377586004\n",
      "\n",
      "sqP = \n",
      "[[0.552690516918679, 0.0, 0.0], [-0.02552813497015995, 0.3019766294167444, 0.0], [-0.13336923614697688, 0.09978975174276535, 0.5007194165184718]]\n",
      "sigma = \n",
      "[[-0.56902668  6.97792155  9.56857756]\n",
      " [ 0.53635435  6.97792155  9.56857756]\n",
      " [-0.62008295  7.58187481  9.56857756]\n",
      " [-0.83576515  7.17750105 10.57001639]\n",
      " [-1.67440771  6.97792155  9.56857756]\n",
      " [-0.51797041  6.37396829  9.56857756]\n",
      " [-0.30228821  6.77834204  8.56713873]]\n",
      "sqP = \n",
      "[[0.7238134057319742, 0.0, 0.0], [-0.008951075205090555, 0.5475574685671264, 0.0], [-0.061550308541668194, 0.08181490439165885, 0.6564052299918978]]\n",
      "m = \n",
      "[-0.61770517  7.0175599   9.63696014]\n",
      "P = \n",
      "[[ 0.31092553 -0.01592436 -0.0740347 ]\n",
      " [-0.01592436  0.09278175  0.03228468]\n",
      " [-0.0740347   0.03228468  0.26782768]]\n",
      "num = \n",
      "705\n",
      "SENSOR 9 2.19331599608 -2.58825731994\n",
      "\n",
      "num = \n",
      "706\n",
      "SENSOR 6 2.6367644419 -0.585514977039\n",
      "\n",
      "num = \n",
      "707\n",
      "ODOMETRY -0.00969850517913 0.100084649032 -0.0200844906424\n",
      "\n",
      "sqP = \n",
      "[[0.5576069663422271, 0.0, 0.0], [-0.028558390234965498, 0.3032592441606202, 0.0], [-0.1327722022993575, 0.0939556447184613, 0.49129579822587693]]\n",
      "sigma = \n",
      "[[-0.61770517  7.0175599   9.63696014]\n",
      " [ 0.49750876  7.0175599   9.63696014]\n",
      " [-0.67482195  7.62407838  9.63696014]\n",
      " [-0.88324957  7.20547119 10.61955174]\n",
      " [-1.7329191   7.0175599   9.63696014]\n",
      " [-0.56058839  6.41104141  9.63696014]\n",
      " [-0.35216076  6.82964861  8.65436855]]\n",
      "sqP = \n",
      "[[0.7275778778038923, 0.0, 0.0], [-0.007336307143639549, 0.5471771966209489, 0.0], [-0.05970810349054294, 0.07494238220763769, 0.6506704630166094]]\n",
      "m = \n",
      "[-0.61579823  7.0491957   9.55357506]\n",
      "P = \n",
      "[[ 0.31235712 -0.01509055 -0.06963402]\n",
      " [-0.01509055  0.09472508  0.02907787]\n",
      " [-0.06963402  0.02907787  0.25126247]]\n",
      "num = \n",
      "708\n",
      "SENSOR 9 2.38239873462 -2.51826314964\n",
      "\n",
      "num = \n",
      "709\n",
      "SENSOR 6 2.55584502849 -0.617585417568\n",
      "\n",
      "num = \n",
      "710\n",
      "ODOMETRY -0.009760488778 0.100025282561 -0.0212037030791\n",
      "\n",
      "sqP = \n",
      "[[0.5588891829477843, 0.0, 0.0], [-0.027000969021517975, 0.3065877087642597, 0.0], [-0.12459360779151352, 0.08387069143809783, 0.47823070818108676]]\n",
      "sigma = \n",
      "[[-0.61579823  7.0491957   9.55357506]\n",
      " [ 0.50198013  7.0491957   9.55357506]\n",
      " [-0.66980017  7.66237111  9.55357506]\n",
      " [-0.86498545  7.21693708 10.51003647]\n",
      " [-1.7335766   7.0491957   9.55357506]\n",
      " [-0.56179629  6.43602028  9.55357506]\n",
      " [-0.36661102  6.88145431  8.59711364]]\n",
      "sqP = \n",
      "[[0.7270555262472382, 0.0, 0.0], [-0.004210641089565147, 0.5476823945842813, 0.0], [-0.052118276961505734, 0.06516338509618834, 0.643276503840716]]\n",
      "m = \n",
      "[-0.60499675  7.17260715  9.54722678]\n",
      "P = \n",
      "[[ 0.31237529 -0.01550789 -0.06953429]\n",
      " [-0.01550789  0.09467303  0.03195433]\n",
      " [-0.06953429  0.03195433  0.2557618 ]]\n",
      "num = \n",
      "711\n",
      "SENSOR 9 2.55229313835 -2.55940451124\n",
      "\n",
      "num = \n",
      "712\n",
      "SENSOR 6 2.63729703758 -0.564546146269\n",
      "\n",
      "num = \n",
      "713\n",
      "ODOMETRY -0.00901024455226 0.0999822225335 -0.0203196475783\n",
      "\n",
      "sqP = \n",
      "[[0.5589054413109477, 0.0, 0.0], [-0.027746889845689602, 0.3064361971735455, 0.0], [-0.12441154564826717, 0.09301218066127336, 0.48128193912321404]]\n",
      "sigma = \n",
      "[[-0.60499675  7.17260715  9.54722678]\n",
      " [ 0.51281413  7.17260715  9.54722678]\n",
      " [-0.66049053  7.78547955  9.54722678]\n",
      " [-0.85381984  7.35863151 10.50979066]\n",
      " [-1.72280763  7.17260715  9.54722678]\n",
      " [-0.54950297  6.55973476  9.54722678]\n",
      " [-0.35617366  6.98658279  8.5846629 ]]\n",
      "sqP = \n",
      "[[0.7270649317039606, 0.0, 0.0], [-0.006396210166552065, 0.5489254007022202, 0.0], [-0.0523072446768933, 0.07312419446230223, 0.6446011456499239]]\n",
      "m = \n",
      "[-0.64114924  7.27855138  9.65569292]\n",
      "P = \n",
      "[[ 0.31288374 -0.01709094 -0.06945821]\n",
      " [-0.01709094  0.09535259  0.03285367]\n",
      " [-0.06945821  0.03285367  0.25367127]]\n",
      "num = \n",
      "714\n",
      "SENSOR 9 2.542229302 -2.53541280471\n",
      "\n",
      "num = \n",
      "715\n",
      "SENSOR 6 2.27227869215 -0.573875753025\n",
      "\n",
      "num = \n",
      "716\n",
      "ODOMETRY -0.00961564138041 0.0999414405798 -0.0204022645065\n",
      "\n",
      "sqP = \n",
      "[[0.5593601170367712, 0.0, 0.0], [-0.030554447012012223, 0.3072767804299657, 0.0], [-0.124174411827846, 0.0945713674482688, 0.4788614055754057]]\n",
      "sigma = \n",
      "[[-0.64114924  7.27855138  9.65569292]\n",
      " [ 0.47757099  7.27855138  9.65569292]\n",
      " [-0.70225814  7.89310495  9.65569292]\n",
      " [-0.88949807  7.46769412 10.61341573]\n",
      " [-1.75986948  7.27855138  9.65569292]\n",
      " [-0.58004035  6.66399782  9.65569292]\n",
      " [-0.39280042  7.08940865  8.6979701 ]]\n",
      "sqP = \n",
      "[[0.7274864942501624, 0.0, 0.0], [-0.006771152070813889, 0.5494881669987843, 0.0], [-0.05244264239288883, 0.0736563880308518, 0.6429292821351167]]\n",
      "m = \n",
      "[-0.6518483   7.41291297  9.51592597]\n",
      "P = \n",
      "[[ 0.31189798 -0.0166903  -0.06366836]\n",
      " [-0.0166903   0.09806294  0.02824761]\n",
      " [-0.06366836  0.02824761  0.23307058]]\n",
      "num = \n",
      "717\n",
      "SENSOR 9 2.70341312756 -2.48331655508\n",
      "\n",
      "num = \n",
      "718\n",
      "SENSOR 6 2.28488250206 -0.552514217678\n",
      "\n",
      "num = \n",
      "719\n",
      "ODOMETRY -0.00990457116932 0.100026465638 -0.0207767178837\n",
      "\n",
      "sqP = \n",
      "[[0.558478271409022, 0.0, 0.0], [-0.02988531928047308, 0.31172072135377654, 0.0], [-0.11400329414712296, 0.0796885789313431, 0.46230245017791716]]\n",
      "sigma = \n",
      "[[-0.6518483   7.41291297  9.51592597]\n",
      " [ 0.46510825  7.41291297  9.51592597]\n",
      " [-0.71161894  8.03635442  9.51592597]\n",
      " [-0.87985488  7.57229013 10.44053087]\n",
      " [-1.76880484  7.41291297  9.51592597]\n",
      " [-0.59207766  6.78947153  9.51592597]\n",
      " [-0.42384171  7.25353582  8.59132107]]\n",
      "sqP = \n",
      "[[0.7251123106256716, 0.0, 0.0], [-0.0029614126633937497, 0.5499376392726916, 0.0], [-0.04388984362490078, 0.05968778536434544, 0.6336802116586095]]\n",
      "m = \n",
      "[-0.67513419  7.5323136   9.51985595]\n",
      "P = \n",
      "[[ 0.31162008 -0.01830744 -0.06459382]\n",
      " [-0.01830744  0.09911659  0.03245134]\n",
      " [-0.06459382  0.03245134  0.23683054]]\n",
      "num = \n",
      "720\n",
      "SENSOR 9 2.56436735907 -2.50931464148\n",
      "\n",
      "num = \n",
      "721\n",
      "SENSOR 6 2.08017880027 -0.487502087503\n",
      "\n",
      "num = \n",
      "722\n",
      "ODOMETRY -0.00998320897741 0.0999813680912 -0.0199150821834\n",
      "\n",
      "sqP = \n",
      "[[0.5582294166857714, 0.0, 0.0], [-0.03279554543258763, 0.3131150656792087, 0.0], [-0.11571196089594546, 0.09152067968021199, 0.46375127906278046]]\n",
      "sigma = \n",
      "[[-0.67513419  7.5323136   9.51985595]\n",
      " [ 0.44132464  7.5323136   9.51985595]\n",
      " [-0.74072528  8.15854373  9.51985595]\n",
      " [-0.90655812  7.71535496 10.44735851]\n",
      " [-1.79159303  7.5323136   9.51985595]\n",
      " [-0.6095431   6.90608347  9.51985595]\n",
      " [-0.44371027  7.34927224  8.59235339]]\n",
      "sqP = \n",
      "[[0.7253170943442201, 0.0, 0.0], [-0.005623252454656384, 0.5522376657579261, 0.0], [-0.045667662769664615, 0.06879545754258556, 0.6336663133346574]]\n",
      "m = \n",
      "[-0.69571646  7.54920288  9.36494574]\n",
      "P = \n",
      "[[ 0.31245097 -0.01997249 -0.0645408 ]\n",
      " [-0.01997249  0.10039017  0.03222056]\n",
      " [-0.0645408   0.03222056  0.23192947]]\n",
      "num = \n",
      "723\n",
      "SENSOR 9 2.658209228 -2.52967300837\n",
      "\n",
      "num = \n",
      "724\n",
      "SENSOR 6 2.08742861878 -0.468280843216\n",
      "\n",
      "num = \n",
      "725\n",
      "ODOMETRY -0.0107252267638 0.10021159563 -0.0192940861751\n",
      "\n",
      "sqP = \n",
      "[[0.5589731363372843, 0.0, 0.0], [-0.035730684548701375, 0.3148229494641987, 0.0], [-0.11546315015322663, 0.08924056727496829, 0.4589486415993559]]\n",
      "sigma = \n",
      "[[-0.69571646  7.54920288  9.36494574]\n",
      " [ 0.42222981  7.54920288  9.36494574]\n",
      " [-0.76717783  8.17884877  9.36494574]\n",
      " [-0.92664276  7.72768401 10.28284302]\n",
      " [-1.81366274  7.54920288  9.36494574]\n",
      " [-0.62425509  6.91955698  9.36494574]\n",
      " [-0.46479016  7.37072174  8.44704845]]\n",
      "sqP = \n",
      "[[0.7259885591442647, 0.0, 0.0], [-0.0059461755501234856, 0.5527230312703424, 0.0], [-0.04512950158216487, 0.06591980132332428, 0.6306766465815611]]\n",
      "m = \n",
      "[-0.71839323  7.59267949  9.31647511]\n",
      "P = \n",
      "[[ 0.31546522 -0.02229267 -0.07203501]\n",
      " [-0.02229267  0.09927441  0.03860126]\n",
      " [-0.07203501  0.03860126  0.2504841 ]]\n",
      "num = \n",
      "726\n",
      "SENSOR 9 2.79062572373 -2.51801590567\n",
      "\n",
      "num = \n",
      "727\n",
      "SENSOR 6 1.98134535694 -0.51239911617\n",
      "\n",
      "num = \n",
      "728\n",
      "ODOMETRY -0.00931898482752 0.0998313946401 -0.0195045943639\n",
      "\n",
      "sqP = \n",
      "[[0.5616629059168847, 0.0, 0.0], [-0.03969047607339264, 0.31256851703356664, 0.0], [-0.12825309383361777, 0.10721114638940184, 0.47174253240108865]]\n",
      "sigma = \n",
      "[[-0.71839323  7.59267949  9.31647511]\n",
      " [ 0.40493258  7.59267949  9.31647511]\n",
      " [-0.79777418  8.21781652  9.31647511]\n",
      " [-0.97489942  7.80710178 10.25996017]\n",
      " [-1.84171904  7.59267949  9.31647511]\n",
      " [-0.63901228  6.96754245  9.31647511]\n",
      " [-0.46188704  7.37825719  8.37299004]]\n",
      "sqP = \n",
      "[[0.7304035938108648, 0.0, 0.0], [-0.011037649758928804, 0.5539937771828181, 0.0], [-0.05510333344835221, 0.08094455200360366, 0.6367890529902052]]\n",
      "m = \n",
      "[-0.72578215  7.69777936  9.26924044]\n",
      "P = \n",
      "[[ 0.32120528 -0.02513602 -0.08050181]\n",
      " [-0.02513602  0.09848627  0.04217466]\n",
      " [-0.08050181  0.04217466  0.26379732]]\n",
      "num = \n",
      "729\n",
      "SENSOR 9 2.97631712843 -2.59124598555\n",
      "\n",
      "num = \n",
      "730\n",
      "SENSOR 6 1.83278194557 -0.45058413267\n",
      "\n",
      "num = \n",
      "731\n",
      "ODOMETRY -0.0102760318389 0.0999106615633 -0.0196442889398\n",
      "\n",
      "sqP = \n",
      "[[0.5667497512455245, 0.0, 0.0], [-0.04435117623948504, 0.31067545864545004, 0.0], [-0.14204119734571927, 0.11547407394746653, 0.47988265035527133]]\n",
      "sigma = \n",
      "[[-0.72578215  7.69777936  9.26924044]\n",
      " [ 0.40771735  7.69777936  9.26924044]\n",
      " [-0.8144845   8.31913028  9.26924044]\n",
      " [-1.00986454  7.92872751 10.22900574]\n",
      " [-1.85928165  7.69777936  9.26924044]\n",
      " [-0.6370798   7.07642844  9.26924044]\n",
      " [-0.44169975  7.46683121  8.30947514]]\n",
      "sqP = \n",
      "[[0.7371214344421789, 0.0, 0.0], [-0.015640777412062223, 0.5540232046524848, 0.0], [-0.06448595814291208, 0.08756872870845972, 0.640320733696868]]\n",
      "m = \n",
      "[-0.76552373  7.85716643  9.21128845]\n",
      "P = \n",
      "[[ 0.32917065 -0.02863465 -0.08819598]\n",
      " [-0.02863465  0.09932869  0.04391883]\n",
      " [-0.08819598  0.04391883  0.26896658]]\n",
      "num = \n",
      "732\n",
      "SENSOR 9 3.00538927132 -2.47582875838\n",
      "\n",
      "num = \n",
      "733\n",
      "SENSOR 5 4.86844686309 -0.644513040378\n",
      "\n",
      "num = \n",
      "734\n",
      "SENSOR 6 1.75818612283 -0.557237052247\n",
      "\n",
      "num = \n",
      "735\n",
      "ODOMETRY -0.0104218597473 0.100145397313 -0.0200408029886\n",
      "\n",
      "sqP = \n",
      "[[0.5737339507126116, 0.0, 0.0], [-0.04990927644213688, 0.3111876557104474, 0.0], [-0.15372278852688984, 0.11647839542227188, 0.4814235888329596]]\n",
      "sigma = \n",
      "[[-0.76552373  7.85716643  9.21128845]\n",
      " [ 0.38194417  7.85716643  9.21128845]\n",
      " [-0.86534228  8.47954174  9.21128845]\n",
      " [-1.07296931  8.09012322 10.17413563]\n",
      " [-1.91299163  7.85716643  9.21128845]\n",
      " [-0.66570518  7.23479112  9.21128845]\n",
      " [-0.45807816  7.62420964  8.24844128]]\n",
      "sqP = \n",
      "[[0.7451793594751278, 0.0, 0.0], [-0.01789618634970054, 0.5539890998530796, 0.0], [-0.07181558362033341, 0.08698105602441276, 0.6404237103386164]]\n",
      "m = \n",
      "[-0.70632924  7.94286093  9.24794265]\n",
      "P = \n",
      "[[ 0.2517282  -0.03082946 -0.05666324]\n",
      " [-0.03082946  0.10158968  0.04543079]\n",
      " [-0.05666324  0.04543079  0.16021533]]\n",
      "num = \n",
      "736\n",
      "SENSOR 9 3.06790211886 -2.40968598481\n",
      "\n",
      "num = \n",
      "737\n",
      "SENSOR 5 4.81572738233 -0.616401204908\n",
      "\n",
      "num = \n",
      "738\n",
      "SENSOR 6 1.68748831042 -0.389912308279\n",
      "\n",
      "num = \n",
      "739\n",
      "ODOMETRY -0.0101350745755 0.100091285908 -0.0206119427088\n",
      "\n",
      "sqP = \n",
      "[[0.5017252241971735, 0.0, 0.0], [-0.06144690738335787, 0.31275223528940965, 0.0], [-0.11293679084675701, 0.12307242763089529, 0.36374962620421736]]\n",
      "sigma = \n",
      "[[-0.70632924  7.94286093  9.24794265]\n",
      " [ 0.29712121  7.94286093  9.24794265]\n",
      " [-0.82922306  8.5683654   9.24794265]\n",
      " [-0.93220282  8.18900578  9.9754419 ]\n",
      " [-1.70977969  7.94286093  9.24794265]\n",
      " [-0.58343543  7.31735645  9.24794265]\n",
      " [-0.48045566  7.69671607  8.52044339]]\n",
      "sqP = \n",
      "[[0.6842942654658499, 0.0, 0.0], [-0.026478189286330472, 0.5558620900966024, 0.0], [-0.034879514607280504, 0.07097100731773257, 0.5667290700343758]]\n",
      "m = \n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[-0.73114475  8.01746128  9.2461337 ]\n",
      "P = \n",
      "[[ 0.22401794 -0.03001752 -0.03954074]\n",
      " [-0.03001752  0.10425294  0.04269655]\n",
      " [-0.03954074  0.04269655  0.13094492]]\n",
      "num = \n",
      "740\n",
      "SENSOR 9 3.05939885103 -2.42259296594\n",
      "\n",
      "num = \n",
      "741\n",
      "SENSOR 5 4.85625275968 -0.554387962083\n",
      "\n",
      "num = \n",
      "742\n",
      "SENSOR 6 1.69976007626 -0.4523568891\n",
      "\n",
      "num = \n",
      "743\n",
      "ODOMETRY -0.0112125747693 0.100293480859 -0.0200290197221\n",
      "\n",
      "sqP = \n",
      "[[0.4733053298704608, 0.0, 0.0], [-0.06342104367994165, 0.31659233492281164, 0.0], [-0.083541722921119, 0.11812743988042664, 0.33167998097684726]]\n",
      "sigma = \n",
      "[[-0.73114475  8.01746128  9.2461337 ]\n",
      " [ 0.21546591  8.01746128  9.2461337 ]\n",
      " [-0.85798683  8.65064595  9.2461337 ]\n",
      " [-0.89822819  8.25371616  9.90949366]\n",
      " [-1.67775541  8.01746128  9.2461337 ]\n",
      " [-0.60430266  7.38427661  9.2461337 ]\n",
      " [-0.5640613   7.7812064   8.58277374]]\n",
      "sqP = \n",
      "[[0.6595599923690549, 0.0, 0.0], [-0.024604733446033443, 0.5572406321561985, 0.0], [-0.019290115790014863, 0.06345201317425465, 0.5501277077035822]]\n",
      "m = \n",
      "[-0.76216723  8.04151403  9.27901364]\n",
      "P = \n",
      "[[ 0.21242301 -0.02851617 -0.0327866 ]\n",
      " [-0.02851617  0.10569325  0.04013258]\n",
      " [-0.0327866   0.04013258  0.1223882 ]]\n",
      "num = \n",
      "744\n",
      "SENSOR 9 3.36060085358 -2.4353356474\n",
      "\n",
      "num = \n",
      "745\n",
      "SENSOR 5 4.60467409396 -0.689671837769\n",
      "\n",
      "num = \n",
      "746\n",
      "SENSOR 6 1.66466459286 -0.434947234979\n",
      "\n",
      "num = \n",
      "747\n",
      "ODOMETRY -0.00936003709598 0.0997788840301 -0.0195655025372\n",
      "\n",
      "sqP = \n",
      "[[0.4608937075886097, 0.0, 0.0], [-0.061871469940010325, 0.31916324239148686, 0.0], [-0.07113701048819321, 0.1119528269249519, 0.3237194664463014]]\n",
      "sigma = \n",
      "[[-0.76216723  8.04151403  9.27901364]\n",
      " [ 0.15962018  8.04151403  9.27901364]\n",
      " [-0.88591017  8.67984051  9.27901364]\n",
      " [-0.90444125  8.26541968  9.92645258]\n",
      " [-1.68395465  8.04151403  9.27901364]\n",
      " [-0.63842429  7.40318754  9.27901364]\n",
      " [-0.61989321  7.81760837  8.63157471]]\n",
      "sqP = \n",
      "[[0.6490851737241498, 0.0, 0.0], [-0.02191938839347516, 0.5576871809431084, 0.0], [-0.014199736279299553, 0.059241017727116714, 0.5466183016329634]]\n",
      "m = \n",
      "[-0.7823567   8.23407491  9.23645204]\n",
      "P = \n",
      "[[ 0.20710165 -0.02702806 -0.02977786]\n",
      " [-0.02702806  0.10634135  0.03828752]\n",
      " [-0.02977786  0.03828752  0.11837291]]\n",
      "num = \n",
      "748\n",
      "SENSOR 9 3.24434042861 -2.50085985671\n",
      "\n",
      "num = \n",
      "749\n",
      "SENSOR 5 4.44097432373 -0.539545027585\n",
      "\n",
      "num = \n",
      "750\n",
      "SENSOR 6 1.44534873942 -0.340798495725\n",
      "\n",
      "num = \n",
      "751\n",
      "ODOMETRY -0.0105609095644 0.100090264715 -0.0197529740061\n",
      "\n",
      "sqP = \n",
      "[[0.45508421767289037, 0.0, 0.0], [-0.059391343823052854, 0.32064624496117805, 0.0], [-0.0654337253186179, 0.107287472790077, 0.3202822712477586]]\n",
      "sigma = \n",
      "[[-0.7823567   8.23407491  9.23645204]\n",
      " [ 0.12781174  8.23407491  9.23645204]\n",
      " [-0.90113938  8.8753674   9.23645204]\n",
      " [-0.91322415  8.44864986  9.87701658]\n",
      " [-1.69252513  8.23407491  9.23645204]\n",
      " [-0.66357401  7.59278242  9.23645204]\n",
      " [-0.65148925  8.01949997  8.5958875 ]]\n",
      "sqP = \n",
      "[[0.6441355053908511, 0.0, 0.0], [-0.019801120562014662, 0.5578526320771447, 0.0], [-0.012132809316248216, 0.05645660465648403, 0.545213465095057]]\n",
      "m = \n",
      "[-0.83341782  8.26746055  9.07998852]\n",
      "P = \n",
      "[[ 0.20623407 -0.02788491 -0.02825947]\n",
      " [-0.02788491  0.10892858  0.03398302]\n",
      " [-0.02825947  0.03398302  0.11191348]]\n",
      "num = \n",
      "752\n",
      "SENSOR 9 3.43181578801 -2.38831289505\n",
      "\n",
      "num = \n",
      "753\n",
      "SENSOR 5 4.47137535155 -0.631269209482\n",
      "\n",
      "num = \n",
      "754\n",
      "SENSOR 6 1.26851574319 -0.398939844547\n",
      "\n",
      "num = \n",
      "755\n",
      "ODOMETRY -0.0107658587575 0.0999709096433 -0.0195658499488\n",
      "\n",
      "sqP = \n",
      "[[0.4541300179175589, 0.0, 0.0], [-0.0614029241100547, 0.32428115013088255, 0.0], [-0.062227704581844547, 0.09301205027900201, 0.31526172508638906]]\n",
      "sigma = \n",
      "[[-0.83341782  8.26746055  9.07998852]\n",
      " [ 0.07484221  8.26746055  9.07998852]\n",
      " [-0.95622367  8.91602285  9.07998852]\n",
      " [-0.95787323  8.45348465  9.71051197]\n",
      " [-1.74167786  8.26746055  9.07998852]\n",
      " [-0.71061197  7.61889825  9.07998852]\n",
      " [-0.70896241  8.08143645  8.44946507]]\n",
      "sqP = \n",
      "[[0.6433324797339186, 0.0, 0.0], [-0.018247166817888266, 0.557380840303152, 0.0], [-0.011198073442869837, 0.04777094542640595, 0.5433710538322635]]\n",
      "m = \n",
      "[-0.85417792  8.39593303  9.03079492]\n",
      "P = \n",
      "[[ 0.20806648 -0.02944311 -0.03139796]\n",
      " [-0.02944311  0.10846885  0.03532998]\n",
      " [-0.03139796  0.03532998  0.11577197]]\n",
      "num = \n",
      "756\n",
      "SENSOR 9 3.50551920675 -2.39732140232\n",
      "\n",
      "num = \n",
      "757\n",
      "SENSOR 5 4.28281556549 -0.589616154158\n",
      "\n",
      "num = \n",
      "758\n",
      "SENSOR 6 1.28831954512 -0.513359363001\n",
      "\n",
      "num = \n",
      "759\n",
      "ODOMETRY -0.0101758354185 0.10002526256 -0.0193481524309\n",
      "\n",
      "sqP = \n",
      "[[0.4561430526032305, 0.0, 0.0], [-0.06454798015549724, 0.32295882745719423, 0.0], [-0.06883359160301913, 0.09563729956454277, 0.31919807161771396]]\n",
      "sigma = \n",
      "[[-0.85417792  8.39593303  9.03079492]\n",
      " [ 0.05810819  8.39593303  9.03079492]\n",
      " [-0.98327388  9.04185069  9.03079492]\n",
      " [-0.9918451   8.58720763  9.66919107]\n",
      " [-1.76646403  8.39593303  9.03079492]\n",
      " [-0.72508196  7.75001538  9.03079492]\n",
      " [-0.71651074  8.20465843  8.39239878]]\n",
      "sqP = \n",
      "[[0.6457329088046537, 0.0, 0.0], [-0.020178196896343464, 0.556732913805887, 0.0], [-0.015018738329156863, 0.04933103723253138, 0.5451981830560293]]\n",
      "m = \n",
      "[-0.85734668  8.49427206  8.96558856]\n",
      "P = \n",
      "[[ 0.2116922  -0.03134171 -0.03284708]\n",
      " [-0.03134171  0.10964211  0.03236422]\n",
      " [-0.03284708  0.03236422  0.11333524]]\n",
      "num = \n",
      "760\n",
      "SENSOR 9 3.67081338849 -2.3789885528\n",
      "\n",
      "num = \n",
      "761\n",
      "SENSOR 5 4.40168128717 -0.489968693872\n",
      "\n",
      "num = \n",
      "762\n",
      "SENSOR 6 1.19631205076 -0.512575604526\n",
      "\n",
      "num = \n",
      "763\n",
      "ODOMETRY -0.0110076328184 0.100028655375 -0.0196243037287\n",
      "\n",
      "sqP = \n",
      "[[0.4601002061278817, 0.0, 0.0], [-0.06811931247855879, 0.32403991204431254, 0.0], [-0.07139113545445125, 0.08486950453549451, 0.31786115014869026]]\n",
      "sigma = \n",
      "[[-0.85734668  8.49427206  8.96558856]\n",
      " [ 0.06285373  8.49427206  8.96558856]\n",
      " [-0.99358531  9.14235188  8.96558856]\n",
      " [-1.00012895  8.66401107  9.60131086]\n",
      " [-1.77754709  8.49427206  8.96558856]\n",
      " [-0.72110806  7.84619224  8.96558856]\n",
      " [-0.71456441  8.32453305  8.32986626]]\n",
      "sqP = \n",
      "[[0.6491757347844623, 0.0, 0.0], [-0.02068178957754716, 0.5555121158997426, 0.0], [-0.015747793410970868, 0.04284556727336009, 0.5448790783824973]]\n",
      "m = \n",
      "[-0.91165635  8.61837683  9.00478325]\n",
      "P = \n",
      "[[ 0.21604401 -0.03272903 -0.03394957]\n",
      " [-0.03272903  0.11022628  0.02907844]\n",
      " [-0.03394957  0.02907844  0.11083645]]\n",
      "num = \n",
      "764\n",
      "SENSOR 9 3.62146176736 -2.30271042095\n",
      "\n",
      "num = \n",
      "765\n",
      "SENSOR 5 4.18233806232 -0.556833847024\n",
      "\n",
      "num = \n",
      "766\n",
      "SENSOR 6 1.01671369638 -0.459323796962\n",
      "\n",
      "num = \n",
      "767\n",
      "ODOMETRY -0.00955580720005 0.0998135945322 -0.0197517141062\n",
      "\n",
      "sqP = \n",
      "[[0.46480534347134467, 0.0, 0.0], [-0.07041448866382821, 0.3244504320920272, 0.0], [-0.07304040814907851, 0.0737719378940184, 0.3163214360647665]]\n",
      "sigma = \n",
      "[[-0.91165635  8.61837683  9.00478325]\n",
      " [ 0.01795434  8.61837683  9.00478325]\n",
      " [-1.05248532  9.26727769  9.00478325]\n",
      " [-1.05773716  8.7659207   9.63742613]\n",
      " [-1.84126703  8.61837683  9.00478325]\n",
      " [-0.77082737  7.96947596  9.00478325]\n",
      " [-0.76557553  8.47083295  8.37214038]]\n",
      "sqP = \n",
      "[[0.6529449507851999, 0.0, 0.0], [-0.019242729692779525, 0.5541163086891226, 0.0], [-0.017211302523757434, 0.03636549036939778, 0.5444896670980787]]\n",
      "m = \n",
      "[-0.91778204  8.6584857   8.8751898 ]\n",
      "P = \n",
      "[[ 0.21930239 -0.03216739 -0.03197138]\n",
      " [-0.03216739  0.11128545  0.02312389]\n",
      " [-0.03197138  0.02312389  0.10314732]]\n",
      "num = \n",
      "768\n",
      "SENSOR 9 3.70417031979 -2.34473876598\n",
      "\n",
      "num = \n",
      "769\n",
      "SENSOR 5 4.17062919093 -0.570876489648\n",
      "\n",
      "num = \n",
      "770\n",
      "SENSOR 6 0.923913647873 -0.520206587019\n",
      "\n",
      "num = \n",
      "771\n",
      "ODOMETRY -0.0091535851559 0.100020863652 -0.0201880877397\n",
      "\n",
      "sqP = \n",
      "[[0.4682973313492078, 0.0, 0.0], [-0.0686901092129456, 0.32644619308302936, 0.0], [-0.06827154235416472, 0.05646968153673357, 0.3087029113577221]]\n",
      "sigma = \n",
      "[[-0.91778204  8.6584857   8.8751898 ]\n",
      " [ 0.01881262  8.6584857   8.8751898 ]\n",
      " [-1.05516226  9.31137808  8.8751898 ]\n",
      " [-1.05432513  8.77142506  9.49259562]\n",
      " [-1.85437671  8.6584857   8.8751898 ]\n",
      " [-0.78040182  8.00559331  8.8751898 ]\n",
      " [-0.78123896  8.54554633  8.25778398]]\n",
      "sqP = \n",
      "[[0.6547379056904317, 0.0, 0.0], [-0.01599193711440154, 0.5536412379156148, 0.0], [-0.014082581291695876, 0.026436922467433486, 0.5410098291535409]]\n",
      "m = \n",
      "[-0.92982934  8.7235762   8.8236303 ]\n",
      "P = \n",
      "[[ 0.22388005 -0.03255077 -0.03433068]\n",
      " [-0.03255077  0.11031246  0.02170428]\n",
      " [-0.03433068  0.02170428  0.10479574]]\n",
      "num = \n",
      "772\n",
      "SENSOR 9 3.69578950794 -2.41413549551\n",
      "\n",
      "num = \n",
      "773\n",
      "SENSOR 5 4.00110518645 -0.494969326567\n",
      "\n",
      "num = \n",
      "774\n",
      "SENSOR 6 0.964762619449 -0.53458348217\n",
      "\n",
      "num = \n",
      "775\n",
      "ODOMETRY -0.010295675802 0.100004015507 -0.0189695088639\n",
      "\n",
      "sqP = \n",
      "[[0.4731596452952847, 0.0, 0.0], [-0.06879448352973394, 0.324930415572604, 0.0], [-0.07255622759119847, 0.05143505025331075, 0.3112647910011889]]\n",
      "sigma = \n",
      "[[-0.92982934  8.7235762   8.8236303 ]\n",
      " [ 0.01648995  8.7235762   8.8236303 ]\n",
      " [-1.0674183   9.37343703  8.8236303 ]\n",
      " [-1.07494179  8.8264463   9.44615988]\n",
      " [-1.87614863  8.7235762   8.8236303 ]\n",
      " [-0.79224037  8.07371537  8.8236303 ]\n",
      " [-0.78471688  8.6207061   8.20110072]]\n",
      "sqP = \n",
      "[[0.6586935076819478, 0.0, 0.0], [-0.014862744473229909, 0.5523160465674822, 0.0], [-0.016180048470394632, 0.02371733662274562, 0.5424163766094047]]\n",
      "m = \n",
      "[-0.94773384  8.75269009  8.75263214]\n",
      "P = \n",
      "[[ 0.22883703 -0.03265736 -0.03589007]\n",
      " [-0.03265736  0.10996527  0.01916193]\n",
      " [-0.03589007  0.01916193  0.10433359]]\n",
      "num = \n",
      "776\n",
      "SENSOR 9 3.69844459713 -2.31420630353\n",
      "\n",
      "num = \n",
      "777\n",
      "SENSOR 5 3.93813832417 -0.404459278547\n",
      "\n",
      "num = \n",
      "778\n",
      "SENSOR 6 0.762339598312 -0.528477333035\n",
      "\n",
      "num = \n",
      "779\n",
      "ODOMETRY -0.00986627304745 0.0999308954255 -0.0194932169016\n",
      "\n",
      "sqP = \n",
      "[[0.47836913872746, 0.0, 0.0], [-0.06826810608096526, 0.3245069146945626, 0.0], [-0.0750258933752044, 0.04326579663947282, 0.3111796466515923]]\n",
      "sigma = \n",
      "[[-9.47733845e-01  8.75269009e+00  8.75263214e+00]\n",
      " [ 9.00443272e-03  8.75269009e+00  8.75263214e+00]\n",
      " [-1.08427006e+00  9.40170392e+00  8.75263214e+00]\n",
      " [-1.09778563e+00  8.83922168e+00  9.37499143e+00]\n",
      " [-1.90447212e+00  8.75269009e+00  8.75263214e+00]\n",
      " [-8.11197633e-01  8.10367626e+00  8.75263214e+00]\n",
      " [-7.97682058e-01  8.66615850e+00  8.13027284e+00]]\n",
      "sqP = \n",
      "[[0.6626661692522724, 0.0, 0.0], [-0.012929305025526458, 0.5515078384661476, 0.0], [-0.0173738683124372, 0.01914785058250499, 0.5424880768919458]]\n",
      "m = \n",
      "[-0.97618119  8.7707943   8.66271138]\n",
      "P = \n",
      "[[ 0.2327988  -0.03286576 -0.03783019]\n",
      " [-0.03286576  0.10937268  0.01818967]\n",
      " [-0.03783019  0.01818967  0.1058699 ]]\n",
      "num = \n",
      "780\n",
      "SENSOR 9 3.97863915035 -2.33817010298\n",
      "\n",
      "num = \n",
      "781\n",
      "SENSOR 5 3.91815869731 -0.375382277621\n",
      "\n",
      "num = \n",
      "782\n",
      "SENSOR 6 0.764457865234 -0.692422479677\n",
      "\n",
      "num = \n",
      "783\n",
      "ODOMETRY -0.0105039330611 0.100062635978 -0.0196007510172\n",
      "\n",
      "sqP = \n",
      "[[0.48249227958030616, 0.0, 0.0], [-0.06811665600192221, 0.3236244803070364, 0.0], [-0.07840580277604806, 0.0397032053489622, 0.3132827517203745]]\n",
      "sigma = \n",
      "[[-0.97618119  8.7707943   8.66271138]\n",
      " [-0.01119663  8.7707943   8.66271138]\n",
      " [-1.11241451  9.41804326  8.66271138]\n",
      " [-1.1329928   8.85020071  9.28927688]\n",
      " [-1.94116575  8.7707943   8.66271138]\n",
      " [-0.83994788  8.12354534  8.66271138]\n",
      " [-0.81936959  8.69138789  8.03614587]]\n",
      "sqP = \n",
      "[[0.6660226336908086, 0.0, 0.0], [-0.011616103376276525, 0.5507674848500512, 0.0], [-0.019565634432676334, 0.01717703784760927, 0.5436447290376728]]\n",
      "m = \n",
      "[-1.02498852  8.93805388  8.65652644]\n",
      "P = \n",
      "[[ 0.23579059 -0.03362779 -0.04033019]\n",
      " [-0.03362779  0.10864225  0.01830254]\n",
      " [-0.04033019  0.01830254  0.10928369]]\n",
      "num = \n",
      "784\n",
      "SENSOR 9 3.99049095935 -2.43100832721\n",
      "\n",
      "num = \n",
      "785\n",
      "SENSOR 5 3.84981410268 -0.339858112469\n",
      "\n",
      "num = \n",
      "786\n",
      "SENSOR 6 0.670707796904 -0.627865459926\n",
      "\n",
      "num = \n",
      "787\n",
      "ODOMETRY -0.0102670061231 0.100220134545 -0.0197649412615\n",
      "\n",
      "sqP = \n",
      "[[0.4855827307720263, 0.0, 0.0], [-0.0692524442712046, 0.32225198987995585, 0.0], [-0.08305524613367092, 0.03894703851495841, 0.3175982468930659]]\n",
      "sigma = \n",
      "[[-1.02498852  8.93805388  8.65652644]\n",
      " [-0.05382306  8.93805388  8.65652644]\n",
      " [-1.16349341  9.58255786  8.65652644]\n",
      " [-1.19109902  9.01594796  9.29172294]\n",
      " [-1.99615399  8.93805388  8.65652644]\n",
      " [-0.88648364  8.2935499   8.65652644]\n",
      " [-0.85887803  8.8601598   8.02132995]]\n",
      "sqP = \n",
      "[[0.6689429447807033, 0.0, 0.0], [-0.01091065106561456, 0.5498165921038114, 0.0], [-0.023250217096564642, 0.01674145979433521, 0.5459816274120959]]\n",
      "m = \n",
      "[-1.1044719   8.99972259  8.61312634]\n",
      "P = \n",
      "[[ 0.24244345 -0.03206581 -0.03953846]\n",
      " [-0.03206581  0.10881573  0.01154316]\n",
      " [-0.03953846  0.01154316  0.10400226]]\n",
      "num = \n",
      "788\n",
      "SENSOR 9 3.97444895632 -2.34925609157\n",
      "\n",
      "num = \n",
      "789\n",
      "SENSOR 5 3.77657198716 -0.365748954501\n",
      "\n",
      "num = \n",
      "790\n",
      "SENSOR 6 0.724620745167 -0.68784406447\n",
      "\n",
      "num = \n",
      "791\n",
      "ODOMETRY -0.00978517807132 0.0997958111892 -0.0209122965862\n",
      "\n",
      "sqP = \n",
      "[[0.4923854679304781, 0.0, 0.0], [-0.06512339388502625, 0.323380068918727, 0.0], [-0.08029980726163097, 0.01952426973437674, 0.3117258520150683]]\n",
      "sigma = \n",
      "[[-1.1044719   8.99972259  8.61312634]\n",
      " [-0.11970097  8.99972259  8.61312634]\n",
      " [-1.23471869  9.64648273  8.61312634]\n",
      " [-1.26507152  9.03877113  9.23657804]\n",
      " [-2.08924284  8.99972259  8.61312634]\n",
      " [-0.97422511  8.35296246  8.61312634]\n",
      " [-0.94387229  8.96067405  7.98967463]]\n",
      "sqP = \n",
      "[[0.6731512196427154, 0.0, 0.0], [-0.004773928511089786, 0.5499405419086385, 0.0], [-0.02303808171793212, 0.006046112473533804, 0.5431924784472207]]\n",
      "m = \n",
      "[-1.144825    9.02348204  8.58887315]\n",
      "P = \n",
      "[[ 0.24616766 -0.03036318 -0.03923672]\n",
      " [-0.03036318  0.10825309  0.00862959]\n",
      " [-0.03923672  0.00862959  0.10253917]]\n",
      "num = \n",
      "792\n",
      "SENSOR 9 4.03143217141 -2.26626563102\n",
      "\n",
      "num = \n",
      "793\n",
      "SENSOR 5 3.46349133261 -0.34264365269\n",
      "\n",
      "num = \n",
      "794\n",
      "SENSOR 6 0.475188697577 -0.867727704814\n",
      "\n",
      "num = \n",
      "795\n",
      "ODOMETRY -0.0108972815108 0.0998936621075 -0.0198820412631\n",
      "\n",
      "sqP = \n",
      "[[0.4961528548075698, 0.0, 0.0], [-0.061197224032906096, 0.32327695500197173, 0.0], [-0.07908191695475263, 0.01172367774212457, 0.3100770551183681]]\n",
      "sigma = \n",
      "[[-1.144825    9.02348204  8.58887315]\n",
      " [-0.15251929  9.02348204  8.58887315]\n",
      " [-1.26721945  9.67003595  8.58887315]\n",
      " [-1.30298883  9.0469294   9.20902726]\n",
      " [-2.13713071  9.02348204  8.58887315]\n",
      " [-1.02243055  8.37692813  8.58887315]\n",
      " [-0.98666117  9.00003468  7.96871904]]\n",
      "sqP = \n",
      "[[0.6754011438784167, 0.0, 0.0], [-0.0012467233849168923, 0.549978709427234, 0.0], [-0.023303760084555456, 0.001978319412563661, 0.5424422182282533]]\n",
      "m = \n",
      "[-1.14567145  9.0979816   8.41490801]\n",
      "P = \n",
      "[[ 0.24777003 -0.02956419 -0.03930156]\n",
      " [-0.02956419  0.10788547  0.00765525]\n",
      " [-0.03930156  0.00765525  0.10243354]]\n",
      "num = \n",
      "796\n",
      "SENSOR 9 4.23985887378 -2.26854947811\n",
      "\n",
      "num = \n",
      "797\n",
      "SENSOR 5 3.41389965628 -0.282744929273\n",
      "\n",
      "num = \n",
      "798\n",
      "SENSOR 6 0.627709996436 -0.94148755275\n",
      "\n",
      "num = \n",
      "799\n",
      "ODOMETRY -0.0092026691367 0.0998946022284 -0.0202789115158\n",
      "\n",
      "sqP = \n",
      "[[0.4977650313531243, 0.0, 0.0], [-0.05939387278940106, 0.323044630870114, 0.0], [-0.07895604703941736, 0.009180601454159451, 0.31002451844526574]]\n",
      "sigma = \n",
      "[[-1.14567145  9.0979816   8.41490801]\n",
      " [-0.15014139  9.0979816   8.41490801]\n",
      " [-1.2644592   9.74407087  8.41490801]\n",
      " [-1.30358355  9.11634281  9.03495704]\n",
      " [-2.14120151  9.0979816   8.41490801]\n",
      " [-1.02688371  8.45189234  8.41490801]\n",
      " [-0.98775936  9.0796204   7.79485897]]\n",
      "sqP = \n",
      "[[0.6764109076049787, 0.0, 0.0], [0.00010913290174589888, 0.5499302716060858, 0.0], [-0.023197922626280433, 0.0006625878704451922, 0.5424329636154662]]\n",
      "m = \n",
      "[-1.19139738  9.22340069  8.35253121]\n",
      "P = \n",
      "[[ 0.24983696 -0.03096326 -0.03884949]\n",
      " [-0.03096326  0.1060014   0.00707037]\n",
      " [-0.03884949  0.00707037  0.10526872]]\n",
      "num = \n",
      "800\n",
      "SENSOR 9 4.11518464247 -2.31925173137\n",
      "\n",
      "num = \n",
      "801\n",
      "SENSOR 5 3.37747667925 -0.382887053434\n",
      "\n",
      "num = \n",
      "802\n",
      "SENSOR 6 0.347854511055 -1.06521018247\n",
      "\n",
      "num = \n",
      "803\n",
      "ODOMETRY -0.00964561978411 0.0999128007624 -0.0203232411783\n",
      "\n",
      "sqP = \n",
      "[[0.49983693435879983, 0.0, 0.0], [-0.06194671327134532, 0.31963103837716783, 0.0], [-0.07772432329150426, 0.007056903624744561, 0.31492514492345397]]\n",
      "sigma = \n",
      "[[-1.19139738  9.22340069  8.35253121]\n",
      " [-0.19172351  9.22340069  8.35253121]\n",
      " [-1.3152908   9.86266277  8.35253121]\n",
      " [-1.34684602  9.2375145   8.9823815 ]\n",
      " [-2.19107124  9.22340069  8.35253121]\n",
      " [-1.06750395  8.58413861  8.35253121]\n",
      " [-1.03594873  9.20928688  7.72268092]]\n",
      "sqP = \n",
      "[[0.6780231756108692, 0.0, 0.0], [0.00021931214625373207, 0.5478170204313199, 0.0], [-0.024368744491520097, -0.0005056971984679148, 0.5453431449683183]]\n",
      "m = \n",
      "[-1.24383292  9.16766545  8.28560758]\n",
      "P = \n",
      "[[ 0.25398001 -0.0305892  -0.03637207]\n",
      " [-0.0305892   0.10353294  0.00383641]\n",
      " [-0.03637207  0.00383641  0.10424088]]\n",
      "num = \n",
      "804\n",
      "SENSOR 9 4.2592528018 -2.25631821864\n",
      "\n",
      "num = \n",
      "805\n",
      "SENSOR 5 3.48949092568 -0.316251464921\n",
      "\n",
      "num = \n",
      "806\n",
      "SENSOR 6 0.418415784686 -1.24819855562\n",
      "\n",
      "num = \n",
      "807\n",
      "ODOMETRY -0.0090889765955 0.100078317535 -0.0208387649645\n",
      "\n",
      "sqP = \n",
      "[[0.5039642947723477, 0.0, 0.0], [-0.06069716340079523, 0.315988591950696, 0.0], [-0.07217191554859717, -0.001722265880919629, 0.3146889434775859]]\n",
      "sigma = \n",
      "[[-1.24383292  9.16766545  8.28560758]\n",
      " [-0.23590433  9.16766545  8.28560758]\n",
      " [-1.36522724  9.79964264  8.28560758]\n",
      " [-1.38817675  9.16422092  8.91498546]\n",
      " [-2.25176151  9.16766545  8.28560758]\n",
      " [-1.12243859  8.53568827  8.28560758]\n",
      " [-1.09948909  9.17110998  7.65622969]]\n",
      "sqP = \n",
      "[[0.6803476621876837, 0.0, 0.0], [0.0029625062976699774, 0.5458605405585011, 0.0], [-0.023211244742952348, -0.005086656569728209, 0.5455218347833367]]\n",
      "m = \n",
      "[-1.30534189  9.23483594  8.33281873]\n",
      "P = \n",
      "[[ 0.25386927 -0.03107924 -0.03680678]\n",
      " [-0.03107924  0.10317485  0.00626179]\n",
      " [-0.03680678  0.00626179  0.10672379]]\n",
      "num = \n",
      "808\n",
      "SENSOR 9 4.38985383683 -2.35866286668\n",
      "\n",
      "num = \n",
      "809\n",
      "SENSOR 5 3.28107289655 -0.276801161352\n",
      "\n",
      "num = \n",
      "810\n",
      "SENSOR 6 0.520377085458 -1.46790593847\n",
      "\n",
      "num = \n",
      "811\n",
      "ODOMETRY -0.00947836960967 0.0999378658775 -0.0205443341273\n",
      "\n",
      "sqP = \n",
      "[[0.5038544126434388, 0.0, 0.0], [-0.061682968357087115, 0.3152301663585655, 0.0], [-0.07305042441442527, 0.005569964938743301, 0.3183651928868371]]\n",
      "sigma = \n",
      "[[-1.30534189  9.23483594  8.33281873]\n",
      " [-0.29763306  9.23483594  8.33281873]\n",
      " [-1.42870782  9.86529627  8.33281873]\n",
      " [-1.45144274  9.24597587  8.96954911]\n",
      " [-2.31305071  9.23483594  8.33281873]\n",
      " [-1.18197595  8.6043756   8.33281873]\n",
      " [-1.15924104  9.22369601  7.69608834]]\n",
      "sqP = \n",
      "[[0.6804486918443842, 0.0, 0.0], [0.002430882296103391, 0.5452958201029363, 0.0], [-0.025893868713405203, -0.001000816719072794, 0.5477242621837113]]\n",
      "m = \n",
      "[-1.3670754   9.27246755  8.11715426]\n",
      "P = \n",
      "[[ 0.22424287 -0.03011759 -0.00298619]\n",
      " [-0.03011759  0.11801478  0.03136025]\n",
      " [-0.00298619  0.03136025  0.11835482]]\n",
      "num = \n",
      "812\n",
      "SENSOR 9 4.49482605041 -2.26889815025\n",
      "\n",
      "num = \n",
      "813\n",
      "SENSOR 5 3.15258240035 -0.336438423951\n",
      "\n",
      "num = \n",
      "814\n",
      "SENSOR 6 0.430980850097 -1.70329062466\n",
      "\n",
      "num = \n",
      "815\n",
      "ODOMETRY -0.00935420041505 0.100110622918 -0.0200634654475\n",
      "\n",
      "sqP = \n",
      "[[0.47354289496624746, 0.0, 0.0], [-0.06360054520699465, 0.33759406845354406, 0.0], [-0.0063060621871278725, 0.09170535971118822, 0.33151950334127067]]\n",
      "sigma = \n",
      "[[-1.3670754   9.27246755  8.11715426]\n",
      " [-0.41998961  9.27246755  8.11715426]\n",
      " [-1.49427649  9.94765569  8.11715426]\n",
      " [-1.37968753  9.45587827  8.78019326]\n",
      " [-2.31416119  9.27246755  8.11715426]\n",
      " [-1.23987431  8.59727941  8.11715426]\n",
      " [-1.35446328  9.08905683  7.45411525]]\n",
      "sqP = \n",
      "[[0.6544674698897948, 0.0, 0.0], [-0.004284394997923446, 0.5654053440989566, 0.0], [0.002590568004146221, 0.05313476535017972, 0.5544053428237967]]\n",
      "m = \n",
      "[-1.42628029  9.41411749  8.11127572]\n",
      "P = \n",
      "[[ 0.24102443 -0.03259409 -0.02429579]\n",
      " [-0.03259409  0.10129739  0.01300867]\n",
      " [-0.02429579  0.01300867  0.11467155]]\n",
      "num = \n",
      "816\n",
      "SENSOR 9 4.56500399918 -2.3031466037\n",
      "\n",
      "num = \n",
      "817\n",
      "SENSOR 5 2.91571056826 -0.232623268414\n",
      "\n",
      "num = \n",
      "818\n",
      "SENSOR 6 0.330244890906 -1.90953716454\n",
      "\n",
      "num = \n",
      "819\n",
      "ODOMETRY -0.010269880911 0.100061171307 -0.0197644075107\n",
      "\n",
      "sqP = \n",
      "[[0.4909423907772871, 0.0, 0.0], [-0.06639085489255872, 0.31127100688398157, 0.0], [-0.04948806282755385, 0.031236815200751448, 0.33353671168743715]]\n",
      "sigma = \n",
      "[[-1.42628029  9.41411749  8.11127572]\n",
      " [-0.44439551  9.41411749  8.11127572]\n",
      " [-1.559062   10.0366595   8.11127572]\n",
      " [-1.52525642  9.47659112  8.77834914]\n",
      " [-2.40816507  9.41411749  8.11127572]\n",
      " [-1.29349858  8.79157547  8.11127572]\n",
      " [-1.32730417  9.35164386  7.44420229]]\n",
      "sqP = \n",
      "[[0.669239303267454, 0.0, 0.0], [-0.0019195500394039741, 0.5433621108701213, 0.0], [-0.020411585701421948, 0.015897033780571102, 0.5572456736884]]\n",
      "m = \n",
      "[-1.48535103  9.38956724  7.83384141]\n",
      "P = \n",
      "[[ 0.2315045  -0.03490524 -0.00710369]\n",
      " [-0.03490524  0.11297124  0.03492361]\n",
      " [-0.00710369  0.03492361  0.12580161]]\n",
      "num = \n",
      "820\n",
      "SENSOR 9 4.69167462854 -2.22658998309\n",
      "\n",
      "num = \n",
      "821\n",
      "SENSOR 5 3.08433569089 -0.337143319208\n",
      "\n",
      "num = \n",
      "822\n",
      "SENSOR 6 0.554762750925 -2.10973524603\n",
      "\n",
      "num = \n",
      "823\n",
      "ODOMETRY -0.00999199793928 0.0999665305154 -0.0202757591957\n",
      "\n",
      "sqP = \n",
      "[[0.48114914333585973, 0.0, 0.0], [-0.07254557779919787, 0.32818954143561707, 0.0], [-0.014764017659813515, 0.10314936716340287, 0.33903368241154225]]\n",
      "sigma = \n",
      "[[-1.48535103  9.38956724  7.83384141]\n",
      " [-0.52305274  9.38956724  7.83384141]\n",
      " [-1.63044218 10.04594632  7.83384141]\n",
      " [-1.51487906  9.59586597  8.51190878]\n",
      " [-2.44764931  9.38956724  7.83384141]\n",
      " [-1.34025987  8.73318815  7.83384141]\n",
      " [-1.45582299  9.1832685   7.15577405]]\n",
      "sqP = \n",
      "[[0.6610486632680763, 0.0, 0.0], [-0.007701336242691101, 0.5612327480316566, 0.0], [-0.005265917528140661, 0.06119806593687328, 0.558070242106237]]\n",
      "m = \n",
      "[-1.63936239  9.49690771  7.87618483]\n",
      "P = \n",
      "[[ 0.23785045 -0.00832422 -0.01732312]\n",
      " [-0.00832422  0.11834063  0.04891021]\n",
      " [-0.01732312  0.04891021  0.13749739]]\n",
      "num = \n",
      "824\n",
      "SENSOR 9 4.76284132902 -2.15126005988\n",
      "\n",
      "num = \n",
      "825\n",
      "SENSOR 5 2.95938070311 -0.158178168659\n",
      "\n",
      "num = \n",
      "826\n",
      "SENSOR 6 0.549503125028 -2.27808301514\n",
      "\n",
      "num = \n",
      "827\n",
      "ODOMETRY -0.00986005097223 0.100173854445 -0.0189859725838\n",
      "\n",
      "sqP = \n",
      "[[0.4876991338901328, 0.0, 0.0], [-0.017068345452392185, 0.3435830367433755, 0.0], [-0.03552009411277469, 0.14058884074172104, 0.34127773882639073]]\n",
      "sigma = \n",
      "[[-1.63936239  9.49690771  7.87618483]\n",
      " [-0.66396412  9.49690771  7.87618483]\n",
      " [-1.67349908 10.18407378  7.87618483]\n",
      " [-1.71040258  9.77808539  8.55874031]\n",
      " [-2.61476066  9.49690771  7.87618483]\n",
      " [-1.6052257   8.80974164  7.87618483]\n",
      " [-1.5683222   9.21573003  7.19362935]]\n",
      "sqP = \n",
      "[[0.6628751395997954, 0.0, 0.0], [0.014205477226188833, 0.5806626950539459, 0.0], [-0.02069504922593917, 0.08101924380080755, 0.5568089822036861]]\n",
      "m = \n",
      "[-1.67614268  9.59776919  7.78920333]\n",
      "P = \n",
      "[[ 0.20278803  0.00940539 -0.0113623 ]\n",
      " [ 0.00940539  0.123761    0.04840835]\n",
      " [-0.0113623   0.04840835  0.13775295]]\n",
      "num = \n",
      "828\n",
      "SENSOR 9 4.62199069994 -2.18236767895\n",
      "\n",
      "num = \n",
      "829\n",
      "SENSOR 5 2.88364841405 -0.211583718625\n",
      "\n",
      "num = \n",
      "830\n",
      "SENSOR 6 0.608371630443 -2.36582694156\n",
      "\n",
      "num = \n",
      "831\n",
      "ODOMETRY -0.0101721448458 0.0998079193853 -0.0198301989047\n",
      "\n",
      "sqP = \n",
      "[[0.45031991884279293, 0.0, 0.0], [0.020886016859906202, 0.35117627200448787, 0.0], [-0.02523161698677617, 0.1393469307077605, 0.3430725079916331]]\n",
      "sigma = \n",
      "[[-1.67614268  9.59776919  7.78920333]\n",
      " [-0.77550284  9.59776919  7.78920333]\n",
      " [-1.63437064 10.30012174  7.78920333]\n",
      " [-1.72660591  9.87646305  8.47534835]\n",
      " [-2.57678251  9.59776919  7.78920333]\n",
      " [-1.71791471  8.89541665  7.78920333]\n",
      " [-1.62567944  9.31907533  7.10305832]]\n",
      "sqP = \n",
      "[[0.6355005031489899, 0.0, 0.0], [0.03380844270747419, 0.5864246910841159, 0.0], [-0.01683810963430095, 0.08069907572181685, 0.5580216802002539]]\n",
      "m = \n",
      "[-1.72844818  9.57052762  7.72126812]\n",
      "P = \n",
      "[[ 0.19098198  0.01407379 -0.00747009]\n",
      " [ 0.01407379  0.12147969  0.04782888]\n",
      " [-0.00747009  0.04782888  0.13658639]]\n",
      "num = \n",
      "832\n",
      "SENSOR 9 4.6158486866 -2.14373953498\n",
      "\n",
      "num = \n",
      "833\n",
      "SENSOR 5 2.58243352082 -0.200858606282\n",
      "\n",
      "num = \n",
      "834\n",
      "SENSOR 6 0.747480535627 -2.41041090005\n",
      "\n",
      "num = \n",
      "835\n",
      "ODOMETRY -0.0103893957533 0.0999764229641 -0.0201432882674\n",
      "\n",
      "sqP = \n",
      "[[0.43701485207546714, 0.0, 0.0], [0.03220438334017578, 0.34704836882407625, 0.0], [-0.017093448761432307, 0.1394023551513741, 0.3418496556320443]]\n",
      "sigma = \n",
      "[[-1.72844818  9.57052762  7.72126812]\n",
      " [-0.85441847  9.57052762  7.72126812]\n",
      " [-1.66403941 10.26462435  7.72126812]\n",
      " [-1.76263508  9.84933233  8.40496743]\n",
      " [-2.60247788  9.57052762  7.72126812]\n",
      " [-1.79285695  8.87643088  7.72126812]\n",
      " [-1.69426128  9.29172291  7.0375688 ]]\n",
      "sqP = \n",
      "[[0.626347578610649, 0.0, 0.0], [0.04057646522985216, 0.5842959967663627, 0.0], [-0.013825134039886713, 0.08093518184080122, 0.5573037630039703]]\n",
      "m = \n",
      "[-1.75430126  9.5477392   7.52196952]\n",
      "P = \n",
      "[[ 0.18725366  0.01620208 -0.00591289]\n",
      " [ 0.01620208  0.12362934  0.05064139]\n",
      " [-0.00591289  0.05064139  0.13671802]]\n",
      "num = \n",
      "836\n",
      "SENSOR 9 4.86345125952 -2.16850372828\n",
      "\n",
      "num = \n",
      "837\n",
      "SENSOR 5 2.53715647378 -0.0219189488566\n",
      "\n",
      "num = \n",
      "838\n",
      "SENSOR 6 0.638025009019 -2.44082398988\n",
      "\n",
      "num = \n",
      "839\n",
      "ODOMETRY -0.0110796247981 0.100123354178 -0.0203009558875\n",
      "\n",
      "sqP = \n",
      "[[0.4327281542573252, 0.0, 0.0], [0.03744171642341109, 0.3496104362322845, 0.0], [-0.013664212724257364, 0.1463142788570457, 0.3392984578410441]]\n",
      "sigma = \n",
      "[[-1.75430126  9.5477392   7.52196952]\n",
      " [-0.88884495  9.5477392   7.52196952]\n",
      " [-1.67941783 10.24696007  7.52196952]\n",
      " [-1.78162969  9.84036776  8.20056643]\n",
      " [-2.61975757  9.5477392   7.52196952]\n",
      " [-1.8291847   8.84851833  7.52196952]\n",
      " [-1.72697284  9.25511064  6.8433726 ]]\n",
      "sqP = \n",
      "[[0.6235721676959743, 0.0, 0.0], [0.04395276652281709, 0.5876187920902695, 0.0], [-0.012573153060253686, 0.0838899767652389, 0.5553221476172299]]\n",
      "m = \n",
      "[-1.78280304  9.65053304  7.48061105]\n",
      "P = \n",
      "[[ 0.18713242  0.01838739 -0.00391066]\n",
      " [ 0.01838739  0.12797703  0.0567547 ]\n",
      " [-0.00391066  0.0567547   0.13450408]]\n",
      "num = \n",
      "840\n",
      "SENSOR 9 4.80447773758 -2.11401632864\n",
      "\n",
      "num = \n",
      "841\n",
      "SENSOR 5 2.52598129404 -0.147975261228\n",
      "\n",
      "num = \n",
      "842\n",
      "SENSOR 6 0.774158398614 -2.52392662998\n",
      "\n",
      "num = \n",
      "843\n",
      "ODOMETRY -0.0105396114623 0.100008052618 -0.0201317969487\n",
      "\n",
      "sqP = \n",
      "[[0.4325880533917406, 0.0, 0.0], [0.04250554966158392, 0.3552046025082268, 0.0], [-0.009040143266381756, 0.16086210739393927, 0.32946280041901643]]\n",
      "sigma = \n",
      "[[-1.78280304  9.65053304  7.48061105]\n",
      " [-0.91762693  9.65053304  7.48061105]\n",
      " [-1.69779194 10.36094225  7.48061105]\n",
      " [-1.80088333  9.97225726  8.13953665]\n",
      " [-2.64797915  9.65053304  7.48061105]\n",
      " [-1.86781414  8.94012384  7.48061105]\n",
      " [-1.76472275  9.32880883  6.82168545]]\n",
      "sqP = \n",
      "[[0.6237153756959704, 0.0, 0.0], [0.04794565128250342, 0.5948272047452113, 0.0], [-0.010689625323234876, 0.08851364942373925, 0.5486529411688833]]\n",
      "m = \n",
      "[-1.82189861  9.68616439  7.45421271]\n",
      "P = \n",
      "[[ 0.18722852  0.01868782 -0.00230645]\n",
      " [ 0.01868782  0.12491912  0.05613815]\n",
      " [-0.00230645  0.05613815  0.13231657]]\n",
      "num = \n",
      "844\n",
      "SENSOR 9 4.92716774584 -2.183105981\n",
      "\n",
      "num = \n",
      "845\n",
      "SENSOR 5 2.5239692479 -0.10125627604\n",
      "\n",
      "num = \n",
      "846\n",
      "SENSOR 6 0.898459257994 -2.5942905006\n",
      "\n",
      "num = \n",
      "847\n",
      "ODOMETRY -0.0101753448007 0.100002913472 -0.0206574320107\n",
      "\n",
      "sqP = \n",
      "[[0.4326991053943084, 0.0, 0.0], [0.043188953391548296, 0.3507902954271369, 0.0], [-0.0053303750750361666, 0.160689625258791, 0.32629282109506696]]\n",
      "sigma = \n",
      "[[-1.82189861  9.68616439  7.45421271]\n",
      " [-0.9565004   9.68616439  7.45421271]\n",
      " [-1.7355207  10.38774498  7.45421271]\n",
      " [-1.83255936 10.00754364  8.10679835]\n",
      " [-2.68729682  9.68616439  7.45421271]\n",
      " [-1.90827651  8.9845838   7.45421271]\n",
      " [-1.81123786  9.36478514  6.80162707]]\n",
      "sqP = \n",
      "[[0.6237966130098276, 0.0, 0.0], [0.048716170495913154, 0.5921530239441435, 0.0], [-0.009697481599614569, 0.08791253290145234, 0.546852302996856]]\n",
      "m = \n",
      "[-1.84312286  9.78220419  7.43418139]\n",
      "P = \n",
      "[[ 0.18742122  0.01909396 -0.00152789]\n",
      " [ 0.01909396  0.12304897  0.0551431 ]\n",
      " [-0.00152789  0.0551431   0.13090839]]\n",
      "num = \n",
      "848\n",
      "SENSOR 9 5.01194140521 -2.08598842578\n",
      "\n",
      "num = \n",
      "849\n",
      "SENSOR 5 2.42112473613 -0.0714350873771\n",
      "\n",
      "num = \n",
      "850\n",
      "SENSOR 6 0.878381860102 -2.66964579914\n",
      "\n",
      "num = \n",
      "851\n",
      "ODOMETRY -0.0103462997287 0.0999423849927 -0.0195755267881\n",
      "\n",
      "sqP = \n",
      "[[0.43292171943187446, 0.0, 0.0], [0.04410487984804364, 0.34799961218295167, 0.0], [-0.0035292492433319058, 0.15890465180656543, 0.3250311429637302]]\n",
      "sigma = \n",
      "[[-1.84312286  9.78220419  7.43418139]\n",
      " [-0.97727942  9.78220419  7.43418139]\n",
      " [-1.7549131  10.47820341  7.43418139]\n",
      " [-1.85018136 10.10001349  8.08424368]\n",
      " [-2.7089663   9.78220419  7.43418139]\n",
      " [-1.93133262  9.08620496  7.43418139]\n",
      " [-1.83606436  9.46439489  6.78411911]]\n",
      "sqP = \n",
      "[[0.6240023326739111, 0.0, 0.0], [0.04937314916465745, 0.5900227417526328, 0.0], [-0.009290890126978032, 0.08686798553706666, 0.5462634617170044]]\n",
      "m = \n",
      "[-1.90021182  9.84399665  7.39150918]\n",
      "P = \n",
      "[[ 0.18760493  0.01923907 -0.00040244]\n",
      " [ 0.01923907  0.11785534  0.05182271]\n",
      " [-0.00040244  0.05182271  0.12922231]]\n",
      "num = \n",
      "852\n",
      "SENSOR 5 2.23964032564 -0.0586867747141\n",
      "\n",
      "num = \n",
      "853\n",
      "SENSOR 6 1.24988717446 -2.58043089475\n",
      "\n",
      "num = \n",
      "854\n",
      "ODOMETRY -0.0107865547639 0.10003293752 -0.0196069887085\n",
      "\n",
      "sqP = \n",
      "[[0.43313384269639, 0.0, 0.0], [0.0444183122485719, 0.3404149779392673, 0.0], [-0.0009291307889372569, 0.1523551614240406, 0.3255907671335139]]\n",
      "sigma = \n",
      "[[-1.90021182  9.84399665  7.39150918]\n",
      " [-1.03394413  9.84399665  7.39150918]\n",
      " [-1.81137519 10.5248266   7.39150918]\n",
      " [-1.90207008 10.14870697  8.04269071]\n",
      " [-2.7664795   9.84399665  7.39150918]\n",
      " [-1.98904844  9.16316669  7.39150918]\n",
      " [-1.89835356  9.53928633  6.74032764]]\n",
      "sqP = \n",
      "[[0.6241624591690658, 0.0, 0.0], [0.049207085352689715, 0.5838013654472751, 0.0], [-0.009395048055998553, 0.08419272794384935, 0.5470111022557018]]\n",
      "m = \n",
      "[-2.00661556 10.0791757   7.25743303]\n",
      "P = \n",
      "[[ 0.23557221  0.03628987 -0.0041969 ]\n",
      " [ 0.03628987  0.18820466  0.05367765]\n",
      " [-0.0041969   0.05367765  0.12832549]]\n",
      "num = \n",
      "855\n",
      "SENSOR 5 1.94979000746 -0.0453900849002\n",
      "\n",
      "num = \n",
      "856\n",
      "SENSOR 6 1.290168152 -2.6828250479\n",
      "\n",
      "num = \n",
      "857\n",
      "ODOMETRY -0.0108544957486 0.100230199128 -0.0189367396772\n",
      "\n",
      "sqP = \n",
      "[[0.48535781266356437, 0.0, 0.0], [0.0747693212064749, 0.4273338344593013, 0.0], [-0.008647026949450874, 0.12712351837724217, 0.3347989433718854]]\n",
      "sigma = \n",
      "[[-2.00661556 10.0791757   7.25743303]\n",
      " [-1.03589993 10.0791757   7.25743303]\n",
      " [-1.85707691 10.93384337  7.25743303]\n",
      " [-2.02390961 10.33342274  7.92703092]\n",
      " [-2.97733118 10.0791757   7.25743303]\n",
      " [-2.1561542   9.22450803  7.25743303]\n",
      " [-1.9893215   9.82492866  6.58783515]]\n",
      "sqP = \n",
      "[[0.664257050231536, 0.0, 0.0], [0.074415589655526, 0.6326764986520137, 0.0], [-0.017752132981149738, 0.06597900080382682, 0.5552340263330191]]\n",
      "m = \n",
      "[-1.75570593  9.83889536  7.00226225]\n",
      "P = \n",
      "[[0.23621333 0.02976752 0.01534453]\n",
      " [0.02976752 0.22120918 0.0369079 ]\n",
      " [0.01534453 0.0369079  0.11489655]]\n",
      "num = \n",
      "858\n",
      "SENSOR 5 2.22911731184 -0.00122688508787\n",
      "\n",
      "num = \n",
      "859\n",
      "SENSOR 6 1.21205806069 -2.64632589698\n",
      "\n",
      "num = \n",
      "860\n",
      "ODOMETRY -0.00993341143727 0.0999418232338 -0.0202662925326\n",
      "\n",
      "sqP = \n",
      "[[0.4860178337273628, 0.0, 0.0], [0.06124779447298649, 0.46632379771284643, 0.0], [0.03157194628004304, 0.07499978259343902, 0.32905135520880197]]\n",
      "sigma = \n",
      "[[-1.75570593  9.83889536  7.00226225]\n",
      " [-0.78367026  9.83889536  7.00226225]\n",
      " [-1.63321034 10.77154295  7.00226225]\n",
      " [-1.69256204  9.98889492  7.66036496]\n",
      " [-2.7277416   9.83889536  7.00226225]\n",
      " [-1.87820152  8.90624776  7.00226225]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      " [-1.81884982  9.68889579  6.34415954]]\n",
      "sqP = \n",
      "[[0.6640492563212113, 0.0, 0.0], [0.07699776688228994, 0.6518661215070917, 0.0], [0.009611987091792331, 0.03695425264980634, 0.553912287414496]]\n",
      "m = \n",
      "[-1.89640548 10.07358493  7.15468067]\n",
      "P = \n",
      "[[0.26096836 0.04348863 0.00575357]\n",
      " [0.04348863 0.22566208 0.05252571]\n",
      " [0.00575357 0.05252571 0.10960942]]\n",
      "num = \n",
      "861\n",
      "SENSOR 4 4.99272912579 -0.274166164371\n",
      "\n",
      "num = \n",
      "862\n",
      "SENSOR 5 1.81092330137 0.0761110639692\n",
      "\n",
      "num = \n",
      "863\n",
      "SENSOR 6 1.36882473059 -2.60089763233\n",
      "\n",
      "num = \n",
      "864\n",
      "ODOMETRY -0.0104826202942 0.0999751788151 -0.0191967893743\n",
      "\n",
      "sqP = \n",
      "[[0.5108506270109445, 0.0, 0.0], [0.0851298370358874, 0.4673489013302392, 0.0], [0.01126272637631934, 0.1103392238472148, 0.3119420168574506]]\n",
      "sigma = \n",
      "[[-1.89640548 10.07358493  7.15468067]\n",
      " [-0.87470423 10.07358493  7.15468067]\n",
      " [-1.72614581 11.00828274  7.15468067]\n",
      " [-1.87388003 10.29426338  7.7785647 ]\n",
      " [-2.91810674 10.07358493  7.15468067]\n",
      " [-2.06666516  9.13888713  7.15468067]\n",
      " [-1.91893094  9.85290649  6.53079663]]\n",
      "sqP = \n",
      "[[0.6843553910627573, 0.0, 0.0], [0.09100724463880636, 0.6570731398916191, 0.0], [-0.005696332792401174, 0.05093555584233913, 0.5431655318119961]]\n",
      "m = \n",
      "[-1.89872341  9.82314636  6.87590479]\n",
      "P = \n",
      "[[ 0.20817228 -0.0282767  -0.00882336]\n",
      " [-0.0282767   0.23727163  0.01908294]\n",
      " [-0.00882336  0.01908294  0.07386009]]\n",
      "num = \n",
      "865\n",
      "SENSOR 4 4.69854535708 -0.223485928377\n",
      "\n",
      "num = \n",
      "866\n",
      "SENSOR 5 1.7001461128 0.0429298228702\n",
      "\n",
      "num = \n",
      "867\n",
      "SENSOR 6 1.30623051674 -2.58399330818\n",
      "\n",
      "num = \n",
      "868\n",
      "ODOMETRY 0.000324810885789 0.0998821776257 -0.000359989641411\n",
      "\n",
      "sqP = \n",
      "[[0.4562590070360047, 0.0, 0.0], [-0.06197509818696245, 0.48314668747761, 0.0], [-0.01933848697324881, 0.037016577321877284, 0.268544004020818]]\n",
      "sigma = \n",
      "[[-1.89872341  9.82314636  6.87590479]\n",
      " [-0.9862054   9.82314636  6.87590479]\n",
      " [-2.02267361 10.78943973  6.87590479]\n",
      " [-1.93740039  9.89717951  7.4129928 ]\n",
      " [-2.81124143  9.82314636  6.87590479]\n",
      " [-1.77477322  8.85685298  6.87590479]\n",
      " [-1.86004644  9.7491132   6.33881679]]\n",
      "sqP = \n",
      "[[0.6421737859607625, 0.0, 0.0], [-0.020623672785564106, 0.6557947866397654, 0.0], [-0.017631354140622908, 0.014771075677532896, 0.5218776141215233]]\n",
      "m = \n",
      "[-1.9148777   9.96128639  6.75803545]\n",
      "P = \n",
      "[[ 0.19555988  0.00608678 -0.00388601]\n",
      " [ 0.00608678  0.21656194  0.02269481]\n",
      " [-0.00388601  0.02269481  0.06556814]]\n",
      "num = \n",
      "869\n",
      "SENSOR 4 5.09005860279 -0.233743736727\n",
      "\n",
      "num = \n",
      "870\n",
      "SENSOR 5 1.58907104446 0.12166808225\n",
      "\n",
      "num = \n",
      "871\n",
      "SENSOR 6 1.50018263441 -2.63967144455\n",
      "\n",
      "num = \n",
      "872\n",
      "ODOMETRY -0.000418164888098 0.0999656435967 -3.68260681419e-05\n",
      "\n",
      "sqP = \n",
      "[[0.4422215235111295, 0.0, 0.0], [0.01376408712202183, 0.46515856752806334, 0.0], [-0.008787473210673864, 0.04904942311965547, 0.2511674284843302]]\n",
      "sigma = \n",
      "[[-1.9148777   9.96128639  6.75803545]\n",
      " [-1.03043466  9.96128639  6.75803545]\n",
      " [-1.88734953 10.89160353  6.75803545]\n",
      " [-1.93245265 10.05938524  7.26037031]\n",
      " [-2.79932075  9.96128639  6.75803545]\n",
      " [-1.94240588  9.03096926  6.75803545]\n",
      " [-1.89730276  9.86318755  6.25570059]]\n",
      "sqP = \n",
      "[[0.6291474752681085, 0.0, 0.0], [0.035096974597897714, 0.6480920248845754, 0.0], [-0.012674144961856407, 0.01845577798381279, 0.5129566075699383]]\n",
      "m = \n",
      "[-1.92881951  9.88138935  6.73929238]\n",
      "P = \n",
      "[[ 0.19487723 -0.04677853 -0.01028349]\n",
      " [-0.04677853  0.26789179  0.0217861 ]\n",
      " [-0.01028349  0.0217861   0.06444301]]\n",
      "num = \n",
      "873\n",
      "SENSOR 4 4.74946984204 -0.126245943621\n",
      "\n",
      "num = \n",
      "874\n",
      "SENSOR 5 1.55519072464 0.0805634344737\n",
      "\n",
      "num = \n",
      "875\n",
      "SENSOR 6 1.63437464886 -2.74620564163\n",
      "\n",
      "num = \n",
      "876\n",
      "ODOMETRY 0.000475115856201 0.100024313407 0.000393598981251\n",
      "\n",
      "sqP = \n",
      "[[0.4414490105768801, 0.0, 0.0], [-0.10596587186737394, 0.5066192091887481, 0.0], [-0.02329485824248626, 0.038130502019010526, 0.24989283453897965]]\n",
      "sigma = \n",
      "[[-1.92881951  9.88138935  6.73929238]\n",
      " [-1.04592149  9.88138935  6.73929238]\n",
      " [-2.14075125 10.89462776  6.73929238]\n",
      " [-1.97540922  9.95765035  7.23907805]\n",
      " [-2.81171753  9.88138935  6.73929238]\n",
      " [-1.71688776  8.86815093  6.73929238]\n",
      " [-1.88222979  9.80512834  6.23950671]]\n",
      "sqP = \n",
      "[[0.6376900856689374, 0.0, 0.0], [-0.058829533519833756, 0.667796918807789, 0.0], [-0.019149433991742286, 0.01393815494046423, 0.5125444535922199]]\n",
      "m = \n",
      "[-1.97956416 10.13529179  6.63496073]\n",
      "P = \n",
      "[[ 0.1906559  -0.00187357 -0.00430711]\n",
      " [-0.00187357  0.22694677  0.02199175]\n",
      " [-0.00430711  0.02199175  0.06414309]]\n",
      "num = \n",
      "877\n",
      "SENSOR 4 4.50322759327 -0.208257404441\n",
      "\n",
      "num = \n",
      "878\n",
      "SENSOR 5 1.40431609203 0.102953443634\n",
      "\n",
      "num = \n",
      "879\n",
      "SENSOR 6 1.92474673868 -2.70532942045\n",
      "\n",
      "num = \n",
      "880\n",
      "SENSOR 7 4.8035115893 -1.34951568347\n",
      "\n",
      "num = \n",
      "881\n",
      "ODOMETRY -8.36142030851e-05 0.0998721448335 -0.000260661559938\n",
      "\n",
      "sqP = \n",
      "[[0.4366416167561569, 0.0, 0.0], [-0.004290869819415582, 0.4763699786171158, 0.0], [-0.009864176548421073, 0.046076426499337964, 0.24884283017487774]]\n",
      "sigma = \n",
      "[[-1.97956416 10.13529179  6.63496073]\n",
      " [-1.10628093 10.13529179  6.63496073]\n",
      " [-1.9881459  11.08803175  6.63496073]\n",
      " [-1.99929251 10.22744465  7.13264639]\n",
      " [-2.85284739 10.13529179  6.63496073]\n",
      " [-1.97098242  9.18255184  6.63496073]\n",
      " [-1.95983581 10.04313894  6.13727507]]\n",
      "sqP = \n",
      "[[0.6251172810171034, 0.0, 0.0], [0.020593538359447758, 0.655320556085723, 0.0], [-0.014564577076032642, 0.017077446653166465, 0.511906075236664]]\n",
      "m = \n",
      "[-1.8753371   9.75813097  6.34682322]\n",
      "P = \n",
      "[[ 0.1597687  -0.02046299 -0.00318946]\n",
      " [-0.02046299  0.11699898 -0.00212549]\n",
      " [-0.00318946 -0.00212549  0.06234142]]\n",
      "num = \n",
      "882\n",
      "SENSOR 4 4.36260790638 -0.247754318384\n",
      "\n",
      "num = \n",
      "883\n",
      "SENSOR 5 1.47019886492 0.0682113654816\n",
      "\n",
      "num = \n",
      "884\n",
      "SENSOR 6 1.77940808395 -2.71217766237\n",
      "\n",
      "num = \n",
      "885\n",
      "SENSOR 7 5.2180857839 -1.31953374687\n",
      "\n",
      "num = \n",
      "886\n",
      "ODOMETRY 0.000197959165217 0.100131767346 0.000281610671693\n",
      "\n",
      "sqP = \n",
      "[[0.39971076447196024, 0.0, 0.0], [-0.05119448409844907, 0.3381983270405019, 0.0], [-0.007979409214878769, -0.007492617158311546, 0.24944260691659265]]\n",
      "sigma = \n",
      "[[-1.8753371   9.75813097  6.34682322]\n",
      " [-1.07591557  9.75813097  6.34682322]\n",
      " [-1.97772607 10.43452763  6.34682322]\n",
      " [-1.89129592  9.74314574  6.84570843]\n",
      " [-2.67475863  9.75813097  6.34682322]\n",
      " [-1.77294813  9.08173432  6.34682322]\n",
      " [-1.85937828  9.77311621  5.847938  ]]\n",
      "sqP = \n",
      "[[0.6020408967904786, 0.0, 0.0], [-0.005559845884788872, 0.5588625627815252, 0.0], [-0.01056882130132532, -0.0034485767154696146, 0.5123371004828988]]\n",
      "m = \n",
      "[-1.81891083  9.8554745   6.34170745]\n",
      "P = \n",
      "[[ 1.55022504e-01 -2.21093941e-02 -5.02136301e-03]\n",
      " [-2.21093941e-02  1.11170322e-01  1.04611406e-04]\n",
      " [-5.02136301e-03  1.04611406e-04  5.74325733e-02]]\n",
      "num = \n",
      "887\n",
      "SENSOR 4 4.44601043546 -0.326392394425\n",
      "\n",
      "num = \n",
      "888\n",
      "SENSOR 5 1.19003310152 0.115920050334\n",
      "\n",
      "num = \n",
      "889\n",
      "SENSOR 6 2.05288544044 -2.83359564739\n",
      "\n",
      "num = \n",
      "890\n",
      "SENSOR 7 4.97368047041 -1.40535275082\n",
      "\n",
      "num = \n",
      "891\n",
      "ODOMETRY -0.000164122759813 0.0999543664159 -0.000303793093368\n",
      "\n",
      "sqP = \n",
      "[[0.39372897319904615, 0.0, 0.0], [-0.05615384073982551, 0.3286595014342768, 0.0], [-0.012753349017925629, -0.0018607042278307494, 0.23930412268499285]]\n",
      "sigma = \n",
      "[[-1.81891083  9.8554745   6.34170745]\n",
      " [-1.03145288  9.8554745   6.34170745]\n",
      " [-1.93121851 10.5127935   6.34170745]\n",
      " [-1.84441753  9.85175309  6.82031569]\n",
      " [-2.60636878  9.8554745   6.34170745]\n",
      " [-1.70660315  9.1981555   6.34170745]\n",
      " [-1.79340413  9.85919591  5.8630992 ]]\n",
      "sqP = \n",
      "[[0.5986137369601385, 0.0, 0.0], [-0.007634523917929418, 0.5528244605547868, 0.0], [-0.010959718506779051, -0.0011618421492899224, 0.5074678069954828]]\n",
      "m = \n",
      "[-1.7684423   9.83184147  6.19746585]\n",
      "P = \n",
      "[[ 0.1536633  -0.0221278  -0.00433013]\n",
      " [-0.0221278   0.10878863 -0.00056648]\n",
      " [-0.00433013 -0.00056648  0.05756308]]\n",
      "num = \n",
      "892\n",
      "SENSOR 4 4.42845729357 -0.199738979168\n",
      "\n",
      "num = \n",
      "893\n",
      "SENSOR 5 1.17917386982 0.18688342145\n",
      "\n",
      "num = \n",
      "894\n",
      "SENSOR 6 1.9596965856 -2.82416076788\n",
      "\n",
      "num = \n",
      "895\n",
      "SENSOR 7 4.92694129534 -1.35478197562\n",
      "\n",
      "num = \n",
      "896\n",
      "ODOMETRY 0.000333398378648 0.0999246910299 -0.000238132530679\n",
      "\n",
      "sqP = \n",
      "[[0.3919991063623646, 0.0, 0.0], [-0.05644858788780185, 0.32496489577398335, 0.0], [-0.011046284491362404, -0.00366200980114614, 0.23964067147696164]]\n",
      "sigma = \n",
      "[[-1.7684423   9.83184147  6.19746585]\n",
      " [-0.98444408  9.83184147  6.19746585]\n",
      " [-1.88133947 10.48177126  6.19746585]\n",
      " [-1.79053487  9.82451745  6.67674719]\n",
      " [-2.55244051  9.83184147  6.19746585]\n",
      " [-1.65554512  9.18191168  6.19746585]\n",
      " [-1.74634973  9.83916549  5.71818451]]\n",
      "sqP = \n",
      "[[0.597471139775373, 0.0, 0.0], [-0.007391167022314881, 0.5506504669590394, 0.0], [-0.009089303185588107, -0.0019275230926944337, 0.5075883045975544]]\n",
      "m = \n",
      "[-1.76627455  9.78638888  6.18293209]\n",
      "P = \n",
      "[[ 0.15501078 -0.02214913 -0.00563008]\n",
      " [-0.02214913  0.10782756 -0.00071858]\n",
      " [-0.00563008 -0.00071858  0.05774728]]\n",
      "num = \n",
      "897\n",
      "SENSOR 4 4.10993462238 -0.218612334131\n",
      "\n",
      "num = \n",
      "898\n",
      "SENSOR 5 1.02261696425 0.152968841424\n",
      "\n",
      "num = \n",
      "899\n",
      "SENSOR 6 2.17175440342 -2.77569049944\n",
      "\n",
      "num = \n",
      "900\n",
      "SENSOR 7 4.9569182734 -1.483064888\n",
      "\n",
      "num = \n",
      "901\n",
      "ODOMETRY 0.000655493141672 0.100066766391 0.000616803666521\n",
      "\n",
      "sqP = \n",
      "[[0.3937140868887396, 0.0, 0.0], [-0.05625690093120739, 0.32351618214094063, 0.0], [-0.01429991230060512, -0.004707806915229838, 0.23983457970086783]]\n",
      "sigma = \n",
      "[[-1.76627455  9.78638888  6.18293209]\n",
      " [-0.97884638  9.78638888  6.18293209]\n",
      " [-1.87878835 10.43342124  6.18293209]\n",
      " [-1.79487437  9.77697327  6.66260125]\n",
      " [-2.55370272  9.78638888  6.18293209]\n",
      " [-1.65376075  9.13935652  6.18293209]\n",
      " [-1.73767473  9.79580449  5.70326293]]\n",
      "sqP = \n",
      "[[0.598648568534848, 0.0, 0.0], [-0.006898850277925816, 0.5498491669555094, 0.0], [-0.010352305804084232, -0.00254694529514471, 0.5076829865613356]]\n",
      "m = \n",
      "[-1.72419529  9.81196933  5.99704847]\n",
      "P = \n",
      "[[ 0.15564159 -0.02208874 -0.00633488]\n",
      " [-0.02208874  0.10810398 -0.00037939]\n",
      " [-0.00633488 -0.00037939  0.05778376]]\n",
      "num = \n",
      "902\n",
      "SENSOR 4 3.89656801687 -0.154099643488\n",
      "\n",
      "num = \n",
      "903\n",
      "SENSOR 5 0.905689372217 0.172402059922\n",
      "\n",
      "num = \n",
      "904\n",
      "SENSOR 6 2.13167650164 -2.75783273628\n",
      "\n",
      "num = \n",
      "905\n",
      "SENSOR 7 4.8549234685 -1.39334612816\n",
      "\n",
      "num = \n",
      "906\n",
      "ODOMETRY -0.000159452291233 0.100045036471 0.000808106642051\n",
      "\n",
      "sqP = \n",
      "[[0.3945143677088643, 0.0, 0.0], [-0.055989687996618145, 0.3239894018939679, 0.0], [-0.016057410189186386, -0.0039459383761191935, 0.23981314572724843]]\n",
      "sigma = \n",
      "[[-1.72419529  9.81196933  5.99704847]\n",
      " [-0.93516656  9.81196933  5.99704847]\n",
      " [-1.83617467 10.45994813  5.99704847]\n",
      " [-1.75631011  9.80407745  6.47667476]\n",
      " [-2.51322403  9.81196933  5.99704847]\n",
      " [-1.61221592  9.16399052  5.99704847]\n",
      " [-1.69208047  9.8198612   5.51742218]]\n",
      "sqP = \n",
      "[[0.5991946861518944, 0.0, 0.0], [-0.006503430020093794, 0.5501296509155709, 0.0], [-0.010069623855169296, -0.0023418341777838667, 0.5076498802944556]]\n",
      "m = \n",
      "[-1.73625818  9.89825235  5.90407631]\n",
      "P = \n",
      "[[ 0.16100436  0.00038408 -0.00534727]\n",
      " [ 0.00038408  0.08565224 -0.00378893]\n",
      " [-0.00534727 -0.00378893  0.05900288]]\n",
      "num = \n",
      "907\n",
      "SENSOR 4 3.94173958574 -0.302887305467\n",
      "\n",
      "num = \n",
      "908\n",
      "SENSOR 5 1.14270798286 0.168760719018\n",
      "\n",
      "num = \n",
      "909\n",
      "SENSOR 6 2.18631073434 -2.81520889899\n",
      "\n",
      "num = \n",
      "910\n",
      "SENSOR 7 5.10636542466 -1.4209085397\n",
      "\n",
      "num = \n",
      "911\n",
      "ODOMETRY 0.00078211331779 0.10000393139 0.000293823867121\n",
      "\n",
      "sqP = \n",
      "[[0.40125349084692724, 0.0, 0.0], [0.0009572001266062934, 0.29266247135996276, 0.0], [-0.013326409576611157, -0.01290281358161475, 0.2421958048045262]]\n",
      "sigma = \n",
      "[[-1.73625818  9.89825235  5.90407631]\n",
      " [-0.9337512   9.89825235  5.90407631]\n",
      " [-1.73434378 10.48357729  5.90407631]\n",
      " [-1.762911    9.87244672  6.38846792]\n",
      " [-2.53876517  9.89825235  5.90407631]\n",
      " [-1.73817258  9.31292741  5.90407631]\n",
      " [-1.70960537  9.92405798  5.4196847 ]]\n",
      "sqP = \n",
      "[[0.6009849193957625, 0.0, 0.0], [0.024469442446803345, 0.5353256091139952, 0.0], [-0.009334907196476288, -0.006360249343171466, 0.5087658577434359]]\n",
      "m = \n",
      "[-1.72468377  9.91887932  5.92068063]\n",
      "P = \n",
      "[[ 0.16121532 -0.01415529 -0.0100761 ]\n",
      " [-0.01415529  0.1029948  -0.00466388]\n",
      " [-0.0100761  -0.00466388  0.05968166]]\n",
      "num = \n",
      "912\n",
      "SENSOR 4 3.90151568357 -0.230804046777\n",
      "\n",
      "num = \n",
      "913\n",
      "SENSOR 5 0.712985585874 0.132988889564\n",
      "\n",
      "num = \n",
      "914\n",
      "SENSOR 6 2.15622970424 -2.85121206477\n",
      "\n",
      "num = \n",
      "915\n",
      "SENSOR 7 4.91179352895 -1.43675629418\n",
      "\n",
      "num = \n",
      "916\n",
      "ODOMETRY -0.000245429391129 0.0999070378738 4.52120481994e-05\n",
      "\n",
      "sqP = \n",
      "[[0.40151627271948415, 0.0, 0.0], [-0.03525457784728509, 0.3189857616383734, 0.0], [-0.025095115776021828, -0.01739449590925688, 0.242382611072279]]\n",
      "sigma = \n",
      "[[-1.72468377  9.91887932  5.92068063]\n",
      " [-0.92165123  9.91887932  5.92068063]\n",
      " [-1.79519293 10.55685084  5.92068063]\n",
      " [-1.77487401  9.88409033  6.40544585]\n",
      " [-2.52771632  9.91887932  5.92068063]\n",
      " [-1.65417462  9.28090779  5.92068063]\n",
      " [-1.67449354  9.95366831  5.43591541]]\n",
      "sqP = \n",
      "[[0.6025678114150717, 0.0, 0.0], [0.006059434166742338, 0.5487889584844691, 0.0], [-0.013797698419815978, -0.008638006299421965, 0.5087925444073411]]\n",
      "m = \n",
      "[-1.71978464  9.84038403  5.83187372]\n",
      "P = \n",
      "[[ 0.16371897 -0.01795655 -0.01073218]\n",
      " [-0.01795655  0.10476405 -0.00503423]\n",
      " [-0.01073218 -0.00503423  0.05948712]]\n",
      "num = \n",
      "917\n",
      "SENSOR 4 3.64288246527 -0.35019983237\n",
      "\n",
      "num = \n",
      "918\n",
      "SENSOR 5 0.799736995544 0.23802034997\n",
      "\n",
      "num = \n",
      "919\n",
      "SENSOR 6 2.48255730306 -2.88618660948\n",
      "\n",
      "num = \n",
      "920\n",
      "SENSOR 7 4.91606982523 -1.51036160945\n",
      "\n",
      "num = \n",
      "921\n",
      "ODOMETRY -0.000170601987707 0.0999805604304 -7.40939785008e-05\n",
      "\n",
      "sqP = \n",
      "[[0.4046220120999841, 0.0, 0.0], [-0.04437858195465239, 0.3206159502265292, 0.0], [-0.02652396048050019, -0.019373099751667917, 0.24167805754514687]]\n",
      "sigma = \n",
      "[[-1.71978464  9.84038403  5.83187372]\n",
      " [-0.91054062  9.84038403  5.83187372]\n",
      " [-1.80854181 10.48161593  5.83187372]\n",
      " [-1.77283257  9.80163783  6.31522983]\n",
      " [-2.52902867  9.84038403  5.83187372]\n",
      " [-1.63102748  9.19915213  5.83187372]\n",
      " [-1.66673672  9.87913023  5.3485176 ]]\n",
      "sqP = \n",
      "[[0.6053031898905842, 0.0, 0.0], [0.0016777242909217676, 0.5493350888888376, 0.0], [-0.014248994689212912, -0.00957099446277172, 0.5084361406744117]]\n",
      "m = \n",
      "[-1.678841    9.84798811  5.64545257]\n",
      "P = \n",
      "[[ 0.16351082 -0.01932779 -0.01138202]\n",
      " [-0.01932779  0.10480916 -0.00445656]\n",
      " [-0.01138202 -0.00445656  0.0612535 ]]\n",
      "num = \n",
      "922\n",
      "SENSOR 4 3.60050259878 -0.29206119154\n",
      "\n",
      "num = \n",
      "923\n",
      "SENSOR 5 0.456888182182 0.189816312354\n",
      "\n",
      "num = \n",
      "924\n",
      "SENSOR 6 2.6445990428 -2.85742387613\n",
      "\n",
      "num = \n",
      "925\n",
      "SENSOR 7 4.75304634682 -1.42550073299\n",
      "\n",
      "num = \n",
      "926\n",
      "ODOMETRY -0.00152630081795 0.0999133992614 -0.000430283518585\n",
      "\n",
      "sqP = \n",
      "[[0.4043647061376309, 0.0, 0.0], [-0.047797907018807324, 0.32019450371637065, 0.0], [-0.028147906598701238, -0.01812013495295837, 0.24522002311534663]]\n",
      "sigma = \n",
      "[[-1.678841    9.84798811  5.64545257]\n",
      " [-0.87011159  9.84798811  5.64545257]\n",
      " [-1.77443682 10.48837712  5.64545257]\n",
      " [-1.73513682  9.81174784  6.13589262]\n",
      " [-2.48757042  9.84798811  5.64545257]\n",
      " [-1.58324519  9.2075991   5.64545257]\n",
      " [-1.62254519  9.88422838  5.15501252]]\n",
      "sqP = \n",
      "[[0.6054649123863747, 0.0, 0.0], [8.021260733227007e-05, 0.5488545596665457, 0.0], [-0.014063548870718269, -0.009279641208515087, 0.510087039597158]]\n",
      "m = \n",
      "[-1.69172989  9.81716781  5.48571896]\n",
      "P = \n",
      "[[ 0.16351657 -0.01956751 -0.01210243]\n",
      " [-0.01956751  0.1032503  -0.00599958]\n",
      " [-0.01210243 -0.00599958  0.06459416]]\n",
      "num = \n",
      "927\n",
      "SENSOR 4 3.606832886 -0.248641284229\n",
      "\n",
      "num = \n",
      "928\n",
      "SENSOR 5 0.495940573578 0.31820180197\n",
      "\n",
      "num = \n",
      "929\n",
      "SENSOR 6 2.62200407047 -2.87985859301\n",
      "\n",
      "num = \n",
      "930\n",
      "SENSOR 7 4.75456323488 -1.62447424499\n",
      "\n",
      "num = \n",
      "931\n",
      "ODOMETRY -0.000217759654145 0.100001221848 -2.15306964727e-05\n",
      "\n",
      "sqP = \n",
      "[[0.4043718271915761, 0.0, 0.0], [-0.04838988724257728, 0.3176613339805223, 0.0], [-0.02992896196152816, -0.023445847371111425, 0.25129406825123113]]\n",
      "sigma = \n",
      "[[-1.69172989  9.81716781  5.48571896]\n",
      " [-0.88298624  9.81716781  5.48571896]\n",
      " [-1.78850967 10.45249048  5.48571896]\n",
      " [-1.75158781  9.77027612  5.9883071 ]\n",
      " [-2.50047355  9.81716781  5.48571896]\n",
      " [-1.59495012  9.18184514  5.48571896]\n",
      " [-1.63187197  9.86405951  4.98313083]]\n",
      "sqP = \n",
      "[[0.6056020959158728, 0.0, 0.0], [0.00025620945183920074, 0.5476080141992914, 0.0], [-0.015431871723846278, -0.012043778466506692, 0.5129749087913474]]\n",
      "m = \n",
      "[-1.70209818  9.78918277  5.45807251]\n",
      "P = \n",
      "[[ 0.16193506 -0.02014872 -0.01213824]\n",
      " [-0.02014872  0.1014792  -0.00707119]\n",
      " [-0.01213824 -0.00707119  0.0673878 ]]\n",
      "num = \n",
      "932\n",
      "SENSOR 4 3.59697409395 -0.279183028245\n",
      "\n",
      "num = \n",
      "933\n",
      "SENSOR 5 0.480705021357 0.34438043649\n",
      "\n",
      "num = \n",
      "934\n",
      "SENSOR 6 2.63552269525 -2.87953212119\n",
      "\n",
      "num = \n",
      "935\n",
      "SENSOR 7 4.92559449029 -1.53430624815\n",
      "\n",
      "num = \n",
      "936\n",
      "ODOMETRY -0.000482506428273 0.0999122174435 -0.000580035297792\n",
      "\n",
      "sqP = \n",
      "[[0.40241156003473916, 0.0, 0.0], [-0.05006992274578596, 0.3145984789247945, 0.0], [-0.030163748168473573, -0.027277569359908386, 0.25638619842018656]]\n",
      "sigma = \n",
      "[[-1.70209818  9.78918277  5.45807251]\n",
      " [-0.89727506  9.78918277  5.45807251]\n",
      " [-1.80223803 10.41837973  5.45807251]\n",
      " [-1.76242568  9.73462763  5.97084491]\n",
      " [-2.5069213   9.78918277  5.45807251]\n",
      " [-1.60195834  9.15998581  5.45807251]\n",
      " [-1.64177069  9.84373791  4.94530011]]\n",
      "sqP = \n",
      "[[0.604443473384346, 0.0, 0.0], [-0.00035546151714283913, 0.5459614575683233, 0.0], [-0.016014764482333622, -0.01414149921409987, 0.515435682876484]]\n",
      "m = \n",
      "[-1.73575029  9.87308281  5.44299723]\n",
      "P = \n",
      "[[ 0.1608438  -0.02041162 -0.01200959]\n",
      " [-0.02041162  0.10081217 -0.00710873]\n",
      " [-0.01200959 -0.00710873  0.06801829]]\n",
      "num = \n",
      "937\n",
      "SENSOR 4 3.47586551117 -0.349238295616\n",
      "\n",
      "num = \n",
      "938\n",
      "SENSOR 5 0.220999951716 0.388622010704\n",
      "\n",
      "num = \n",
      "939\n",
      "SENSOR 6 2.82004661589 -2.92802181092\n",
      "\n",
      "num = \n",
      "940\n",
      "SENSOR 7 4.82070833488 -1.58213543982\n",
      "\n",
      "num = \n",
      "941\n",
      "ODOMETRY -0.000139550189593 0.100064120469 -2.76254777159e-05\n",
      "\n",
      "sqP = \n",
      "[[0.4010533658235942, 0.0, 0.0], [-0.05089502919029994, 0.3134036857158782, 0.0], [-0.029945112652983323, -0.02754527384206924, 0.25760985596599006]]\n",
      "sigma = \n",
      "[[-1.73575029  9.87308281  5.44299723]\n",
      " [-0.93364356  9.87308281  5.44299723]\n",
      " [-1.83754035 10.49989018  5.44299723]\n",
      " [-1.79564051  9.81799226  5.95821694]\n",
      " [-2.53785702  9.87308281  5.44299723]\n",
      " [-1.63396023  9.24627544  5.44299723]\n",
      " [-1.67586006  9.92817336  4.92777751]]\n",
      "sqP = \n",
      "[[0.603598223992181, 0.0, 0.0], [-0.0009570586824135907, 0.5452492256800947, 0.0], [-0.016804478826384434, -0.01432485843623498, 0.5160731585832611]]\n",
      "m = \n",
      "[-1.7585005   9.87328079  5.31234553]\n",
      "P = \n",
      "[[ 0.1617972  -0.02033876 -0.0122522 ]\n",
      " [-0.02033876  0.10066334 -0.00803046]\n",
      " [-0.0122522  -0.00803046  0.06828649]]\n",
      "num = \n",
      "942\n",
      "SENSOR 4 3.36308480601 -0.338986772029\n",
      "\n",
      "num = \n",
      "943\n",
      "SENSOR 5 0.444200909715 0.716185023584\n",
      "\n",
      "num = \n",
      "944\n",
      "SENSOR 6 2.87114105271 -2.87495975842\n",
      "\n",
      "num = \n",
      "945\n",
      "SENSOR 7 5.00458709072 -1.58257275001\n",
      "\n",
      "num = \n",
      "946\n",
      "ODOMETRY 0.000625023111235 0.100033704567 0.000788974348719\n",
      "\n",
      "sqP = \n",
      "[[0.4022402308336725, 0.0, 0.0], [-0.05056370924861688, 0.313219805932075, 0.0], [-0.03045991342089155, -0.030555638354669224, 0.2577305568077993]]\n",
      "sigma = \n",
      "[[-1.7585005   9.87328079  5.31234553]\n",
      " [-0.95402004  9.87328079  5.31234553]\n",
      " [-1.85962792 10.49972041  5.31234553]\n",
      " [-1.81942032  9.81216952  5.82780664]\n",
      " [-2.56298096  9.87328079  5.31234553]\n",
      " [-1.65737308  9.24684118  5.31234553]\n",
      " [-1.69758067  9.93439207  4.79688441]]\n",
      "sqP = \n",
      "[[0.6043853889032337, 0.0, 0.0], [-0.000530180557531313, 0.545351344221115, 0.0], [-0.017576702629204133, -0.01575226289916403, 0.5161156789001706]]\n",
      "m = \n",
      "[-1.86375316  9.87613189  5.25972708]\n",
      "P = \n",
      "[[ 0.15755178 -0.02097789  0.0099149 ]\n",
      " [-0.02097789  0.10117198  0.00679039]\n",
      " [ 0.0099149   0.00679039  0.06830572]]\n",
      "num = \n",
      "947\n",
      "SENSOR 4 3.13679295783 -0.355723706289\n",
      "\n",
      "num = \n",
      "948\n",
      "SENSOR 5 0.132273645824 1.00432805138\n",
      "\n",
      "num = \n",
      "949\n",
      "SENSOR 6 3.04078654628 -2.86651045039\n",
      "\n",
      "num = \n",
      "950\n",
      "SENSOR 7 4.91504091129 -1.62691182927\n",
      "\n",
      "num = \n",
      "951\n",
      "ODOMETRY -0.000430603052429 0.100031372434 -0.000194224656002\n",
      "\n",
      "sqP = \n",
      "[[0.39692792904978014, 0.0, 0.0], [-0.05285063060888313, 0.3136539345896843, 0.0], [0.024979104748926027, 0.025858274227332907, 0.25886891283016905]]\n",
      "sigma = \n",
      "[[-1.86375316  9.87613189  5.25972708]\n",
      " [-1.06989731  9.87613189  5.25972708]\n",
      " [-1.96945442 10.50343976  5.25972708]\n",
      " [-1.81379495  9.92784844  5.77746491]\n",
      " [-2.65760902  9.87613189  5.25972708]\n",
      " [-1.7580519   9.24882402  5.25972708]\n",
      " [-1.91371137  9.82441534  4.74198926]]\n",
      "sqP = \n",
      "[[0.6008068955076679, 0.0, 0.0], [-0.0034090885355730553, 0.5451614204080297, 0.0], [0.0038086496399985575, 0.013701550044510104, 0.5164282032427446]]\n",
      "m = \n",
      "[-1.96758172  9.83420193  5.09511791]\n",
      "P = \n",
      "[[ 0.15767335 -0.02068366  0.01377101]\n",
      " [-0.02068366  0.10118495  0.00903004]\n",
      " [ 0.01377101  0.00903004  0.06873633]]\n",
      "num = \n",
      "952\n",
      "SENSOR 4 2.89257924978 -0.261154689677\n",
      "\n",
      "num = \n",
      "953\n",
      "SENSOR 5 0.0869154821736 1.57743143981\n",
      "\n",
      "num = \n",
      "954\n",
      "SENSOR 6 2.90779553154 -2.87205172826\n",
      "\n",
      "num = \n",
      "955\n",
      "SENSOR 7 4.87504647749 -1.67682352072\n",
      "\n",
      "num = \n",
      "956\n",
      "ODOMETRY -0.000394366473818 0.0999652691747 0.000203739446394\n",
      "\n",
      "sqP = \n",
      "[[0.39708104098001595, 0.0, 0.0], [-0.05208927104810002, 0.31380194267009315, 0.0], [0.03468060383863671, 0.034533006751132925, 0.2575675729668277]]\n",
      "sigma = \n",
      "[[-1.96758172  9.83420193  5.09511791]\n",
      " [-1.17341964  9.83420193  5.09511791]\n",
      " [-2.07176026 10.46180581  5.09511791]\n",
      " [-1.89822051  9.90326794  5.61025305]\n",
      " [-2.7617438   9.83420193  5.09511791]\n",
      " [-1.86340318  9.20659804  5.09511791]\n",
      " [-2.03694293  9.76513591  4.57998276]]\n",
      "sqP = \n",
      "[[0.6013230326095184, 0.0, 0.0], [-0.002867780971013069, 0.54589996986977, 0.0], [0.005489862720519953, 0.01815282074723114, 0.5153903537419768]]\n",
      "m = \n",
      "[-2.03444782  9.77097535  5.00373264]\n",
      "P = \n",
      "[[ 0.1582902  -0.02084503  0.01380897]\n",
      " [-0.02084503  0.0998314   0.00885675]\n",
      " [ 0.01380897  0.00885675  0.07076409]]\n",
      "num = \n",
      "957\n",
      "SENSOR 4 2.95060694926 -0.40938842696\n",
      "\n",
      "num = \n",
      "958\n",
      "SENSOR 5 0.187834365445 2.26857479846\n",
      "\n",
      "num = \n",
      "959\n",
      "SENSOR 6 3.31126955451 -2.92453711135\n",
      "\n",
      "num = \n",
      "960\n",
      "SENSOR 7 5.00762306838 -1.62193179913\n",
      "\n",
      "num = \n",
      "961\n",
      "ODOMETRY 0.000188872633703 0.0998900920204 -0.000404695062714\n",
      "\n",
      "sqP = \n",
      "[[0.3978570138964463, 0.0, 0.0], [-0.052393278912421286, 0.31158682142661476, 0.0], [0.03470837890092774, 0.034260869135350897, 0.2615064267142995]]\n",
      "sigma = \n",
      "[[-2.03444782  9.77097535  5.00373264]\n",
      " [-1.2387338   9.77097535  5.00373264]\n",
      " [-2.13923438 10.39414899  5.00373264]\n",
      " [-1.96503107  9.83949709  5.52674549]\n",
      " [-2.83016185  9.77097535  5.00373264]\n",
      " [-1.92966127  9.1478017   5.00373264]\n",
      " [-2.10386458  9.70245361  4.48071979]]\n",
      "sqP = \n",
      "[[0.6018637144302041, 0.0, 0.0], [-0.0034556933039634895, 0.5446388674524907, 0.0], [0.004223437052556784, 0.018362783848259706, 0.5173152265812558]]\n",
      "m = \n",
      "[-1.86552535  9.84538942  4.90061007]\n",
      "P = \n",
      "[[ 0.12891043  0.01049929 -0.01093008]\n",
      " [ 0.01049929  0.09999769  0.00777776]\n",
      " [-0.01093008  0.00777776  0.07103949]]\n",
      "num = \n",
      "962\n",
      "SENSOR 4 2.71804063576 -0.263896340835\n",
      "\n",
      "num = \n",
      "963\n",
      "SENSOR 5 0.117592609818 2.55977634237\n",
      "\n",
      "num = \n",
      "964\n",
      "SENSOR 6 3.49353272518 -2.94938522616\n",
      "\n",
      "num = \n",
      "965\n",
      "SENSOR 7 4.88923424474 -1.71729987801\n",
      "\n",
      "num = \n",
      "966\n",
      "ODOMETRY 0.000463123137535 0.100026190262 -0.000467923097136\n",
      "\n",
      "sqP = \n",
      "[[0.35904098659542055, 0.0, 0.0], [0.029242584451956412, 0.3148691098035622, 0.0], [-0.030442429160658036, 0.027528812151305555, 0.2633532146332466]]\n",
      "sigma = \n",
      "[[-1.86552535  9.84538942  4.90061007]\n",
      " [-1.14744337  9.84538942  4.90061007]\n",
      " [-1.80704018 10.47512764  4.90061007]\n",
      " [-1.9264102   9.90044705  5.4273165 ]\n",
      " [-2.58360732  9.84538942  4.90061007]\n",
      " [-1.92401051  9.2156512   4.90061007]\n",
      " [-1.80464049  9.7903318   4.37390364]]\n",
      "sqP = \n",
      "[[0.5750585190170167, 0.0, 0.0], [0.03448360971076234, 0.5489308259392817, 0.0], [-0.019987235195810048, 0.012127351436781808, 0.5191040284681963]]\n",
      "m = \n",
      "[-1.77313775  9.78679085  4.72481958]\n",
      "P = \n",
      "[[ 0.12277585  0.01677034 -0.01180599]\n",
      " [ 0.01677034  0.10072072  0.00647556]\n",
      " [-0.01180599  0.00647556  0.07265602]]\n",
      "num = \n",
      "967\n",
      "SENSOR 4 2.79334888537 -0.399856478188\n",
      "\n",
      "num = \n",
      "968\n",
      "SENSOR 5 0.493078840954 2.63845120424\n",
      "\n",
      "num = \n",
      "969\n",
      "SENSOR 6 3.4746419933 -2.96022159027\n",
      "\n",
      "num = \n",
      "970\n",
      "SENSOR 7 4.852250925 -1.62247582516\n",
      "\n",
      "num = \n",
      "971\n",
      "ODOMETRY 0.000204722786588 0.100025452409 -0.000621143247794\n",
      "\n",
      "sqP = \n",
      "[[0.35039385509643006, 0.0, 0.0], [0.047861403388731405, 0.31373555919945373, 0.0], [-0.03369347776356616, 0.02578024933942413, 0.266188190159717]]\n",
      "sigma = \n",
      "[[-1.77313775  9.78679085  4.72481958]\n",
      " [-1.07235004  9.78679085  4.72481958]\n",
      " [-1.67741494 10.41426197  4.72481958]\n",
      " [-1.84052471  9.83835135  5.25719596]\n",
      " [-2.47392546  9.78679085  4.72481958]\n",
      " [-1.86886056  9.15931973  4.72481958]\n",
      " [-1.7057508   9.73523035  4.1924432 ]]\n",
      "sqP = \n",
      "[[0.5711408040645326, 0.0, 0.0], [0.04474031614420593, 0.5485402497378139, 0.0], [-0.019790553967431283, 0.01165888805446202, 0.5204126545562798]]\n",
      "m = \n",
      "[-1.69449517  9.7525927   4.63267781]\n",
      "P = \n",
      "[[ 0.11250209  0.02490861  0.0110583 ]\n",
      " [ 0.02490861  0.09707411 -0.00581301]\n",
      " [ 0.0110583  -0.00581301  0.07304624]]\n",
      "num = \n",
      "972\n",
      "SENSOR 4 2.73084876629 -0.369067978649\n",
      "\n",
      "num = \n",
      "973\n",
      "SENSOR 5 0.42617115042 2.86523242658\n",
      "\n",
      "num = \n",
      "974\n",
      "SENSOR 6 3.81253344685 -2.96532036628\n",
      "\n",
      "num = \n",
      "975\n",
      "SENSOR 7 4.96857297672 -1.68391105412\n",
      "\n",
      "num = \n",
      "976\n",
      "ODOMETRY -0.000217736658741 0.0999861864311 -0.000591307921892\n",
      "\n",
      "sqP = \n",
      "[[0.3354133195556749, 0.0, 0.0], [0.07426244180250227, 0.30258751052073934, 0.0], [0.03296916785328443, -0.02730244041256824, 0.2668592316686755]]\n",
      "sigma = \n",
      "[[-1.69449517  9.7525927   4.63267781]\n",
      " [-1.02366853  9.7525927   4.63267781]\n",
      " [-1.54597029 10.35776772  4.63267781]\n",
      " [-1.62855683  9.69798782  5.16639628]\n",
      " [-2.36532181  9.7525927   4.63267781]\n",
      " [-1.84302005  9.14741768  4.63267781]\n",
      " [-1.7604335   9.80719758  4.09895935]]\n",
      "sqP = \n",
      "[[0.5648928847372441, 0.0, 0.0], [0.05766126052383635, 0.5424608303434751, 0.0], [0.013157421775421476, -0.013922608797278928, 0.52031657631494]]\n",
      "m = \n",
      "[-1.58826799  9.81169968  4.4670814 ]\n",
      "P = \n",
      "[[ 0.11394279  0.02673721  0.01703205]\n",
      " [ 0.02673721  0.09633108 -0.00731238]\n",
      " [ 0.01703205 -0.00731238  0.07116119]]\n",
      "num = \n",
      "977\n",
      "SENSOR 4 2.6677545575 -0.3747009033\n",
      "\n",
      "num = \n",
      "978\n",
      "SENSOR 5 0.7159982817 2.86732646754\n",
      "\n",
      "num = \n",
      "979\n",
      "SENSOR 6 3.60775134837 -3.03170946031\n",
      "\n",
      "num = \n",
      "980\n",
      "SENSOR 7 4.85926199224 -1.74229396012\n",
      "\n",
      "num = \n",
      "981\n",
      "ODOMETRY -0.000184559437448 0.100062080709 -0.000284379235048\n",
      "\n",
      "sqP = \n",
      "[[0.33755412897538356, 0.0, 0.0], [0.0792086599371094, 0.3000950981422255, 0.0], [0.050457250495354426, -0.03768482960144127, 0.2592201914388225]]\n",
      "sigma = \n",
      "[[-1.58826799  9.81169968  4.4670814 ]\n",
      " [-0.91315973  9.81169968  4.4670814 ]\n",
      " [-1.42985067 10.41188987  4.4670814 ]\n",
      " [-1.48735349  9.73633002  4.98552178]\n",
      " [-2.26337625  9.81169968  4.4670814 ]\n",
      " [-1.74668531  9.21150948  4.4670814 ]\n",
      " [-1.68918249  9.88706934  3.94864102]]\n",
      "sqP = \n",
      "[[0.568122113567949, 0.0, 0.0], [0.058600945924015824, 0.5417193679343988, 0.0], [0.022666669573902867, -0.018178071400638245, 0.516130449628192]]\n",
      "m = \n",
      "[-1.496986    9.70780552  4.45096075]\n",
      "P = \n",
      "[[ 0.1184516   0.02609782  0.01968242]\n",
      " [ 0.02609782  0.09574356 -0.007532  ]\n",
      " [ 0.01968242 -0.007532    0.06803505]]\n",
      "num = \n",
      "982\n",
      "SENSOR 4 2.47524626759 -0.497455287779\n",
      "\n",
      "num = \n",
      "983\n",
      "SENSOR 5 0.515976055959 2.86747266043\n",
      "\n",
      "num = \n",
      "984\n",
      "SENSOR 6 3.71140327246 -2.89739069321\n",
      "\n",
      "num = \n",
      "985\n",
      "SENSOR 7 4.86833979771 -1.86747905373\n",
      "\n",
      "num = \n",
      "986\n",
      "ODOMETRY -0.000423831010905 0.100055729458 0.000522719071377\n",
      "\n",
      "sqP = \n",
      "[[0.34416797750286926, 0.0, 0.0], [0.07582872837574615, 0.2999892656139354, 0.0], [0.05718839770850428, -0.03956317626198446, 0.2513946870171257]]\n",
      "sigma = \n",
      "[[-1.496986    9.70780552  4.45096075]\n",
      " [-0.80865005  9.70780552  4.45096075]\n",
      " [-1.34532855 10.30778405  4.45096075]\n",
      " [-1.38260921  9.62867917  4.95375012]\n",
      " [-2.18532196  9.70780552  4.45096075]\n",
      " [-1.64864346  9.10782699  4.45096075]\n",
      " [-1.6113628   9.78693187  3.94817137]]\n",
      "sqP = \n",
      "[[0.5722517851592638, 0.0, 0.0], [0.056428561932494466, 0.5417943441271937, 0.0], [0.026645030368780784, -0.018076742436026503, 0.5123198468484187]]\n",
      "m = \n",
      "[-1.35313902  9.73357948  4.37659173]\n",
      "P = \n",
      "[[ 0.1314299   0.05202064  0.01537119]\n",
      " [ 0.05202064  0.07057739 -0.00739502]\n",
      " [ 0.01537119 -0.00739502  0.06793551]]\n",
      "num = \n",
      "987\n",
      "SENSOR 4 2.40614223049 -0.363441019406\n",
      "\n",
      "num = \n",
      "988\n",
      "SENSOR 5 0.586364241415 2.87684153835\n",
      "\n",
      "num = \n",
      "989\n",
      "SENSOR 6 3.90284744099 -2.93101327542\n",
      "\n",
      "num = \n",
      "990\n",
      "SENSOR 7 4.77611939003 -1.8279190371\n",
      "\n",
      "num = \n",
      "991\n",
      "ODOMETRY -0.000503367053185 0.100103555799 0.000819633507756\n",
      "\n",
      "sqP = \n",
      "[[0.36253261299820794, 0.0, 0.0], [0.14349229168440342, 0.2235785123457745, 0.0], [0.042399471831315955, -0.06028763986276505, 0.2500063850186893]]\n",
      "sigma = \n",
      "[[-1.35313902  9.73357948  4.37659173]\n",
      " [-0.6280738   9.73357948  4.37659173]\n",
      " [-1.06615444 10.18073651  4.37659173]\n",
      " [-1.26834008  9.6130042   4.8766045 ]\n",
      " [-2.07820425  9.73357948  4.37659173]\n",
      " [-1.64012361  9.28642246  4.37659173]\n",
      " [-1.43793797  9.85415476  3.87657896]]\n",
      "sqP = \n",
      "[[0.5948257294970729, 0.0, 0.0], [0.0730139925984551, 0.5052111725815145, 0.0], [0.022980164806511716, -0.02936084989009503, 0.5115991628139781]]\n",
      "m = \n",
      "[-1.39964571  9.95506883  4.08810729]\n",
      "P = \n",
      "[[ 0.17510859  0.03212121  0.01354719]\n",
      " [ 0.03212121  0.083208   -0.00335026]\n",
      " [ 0.01354719 -0.00335026  0.06475565]]\n",
      "num = \n",
      "992\n",
      "SENSOR 4 2.40160042935 -0.389154949411\n",
      "\n",
      "num = \n",
      "993\n",
      "SENSOR 5 0.853827928964 3.00466868631\n",
      "\n",
      "num = \n",
      "994\n",
      "SENSOR 6 3.82496452269 -2.95364332599\n",
      "\n",
      "num = \n",
      "995\n",
      "SENSOR 7 5.02564796909 -1.78367151428\n",
      "\n",
      "num = \n",
      "996\n",
      "ODOMETRY -0.00080944386785 0.100013043879 1.97022648298e-05\n",
      "\n",
      "sqP = \n",
      "[[0.41845977949517044, 0.0, 0.0], [0.07676057451901223, 0.27805722255543236, 0.0], [0.03237394339908166, -0.020985990509549322, 0.2515296435240761]]\n",
      "sigma = \n",
      "[[-1.39964571  9.95506883  4.08810729]\n",
      " [-0.56272615  9.95506883  4.08810729]\n",
      " [-1.24612456 10.51118328  4.08810729]\n",
      " [-1.33489782  9.91309685  4.59116658]\n",
      " [-2.23656527  9.95506883  4.08810729]\n",
      " [-1.55316686  9.39895439  4.08810729]\n",
      " [-1.4643936   9.99704081  3.58504801]]\n",
      "sqP = \n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[0.6181009990825468, 0.0, 0.0], [0.059333314371591124, 0.5288769763712635, 0.0], [0.017637608230487663, -0.00938424126871848, 0.5132057835882593]]\n",
      "m = \n",
      "[-1.36999368  9.8788354   4.147328  ]\n",
      "P = \n",
      "[[ 0.15234821  0.05456532  0.01646354]\n",
      " [ 0.05456532  0.06286956 -0.00135264]\n",
      " [ 0.01646354 -0.00135264  0.05818964]]\n",
      "num = \n",
      "997\n",
      "SENSOR 4 2.14265941231 -0.576114496391\n",
      "\n",
      "num = \n",
      "998\n",
      "SENSOR 5 1.06875104344 3.01158775394\n",
      "\n",
      "num = \n",
      "999\n",
      "SENSOR 6 4.04413273461 -2.9178294235\n",
      "\n",
      "num = \n",
      "1000\n",
      "SENSOR 7 5.0221824592 -1.79251846775\n",
      "\n",
      "num = \n",
      "1001\n",
      "ODOMETRY -0.000993208621031 0.0998844685408 -0.000121644357505\n",
      "\n",
      "sqP = \n",
      "[[0.3903180823768112, 0.0, 0.0], [0.1397970688254966, 0.20814981058832924, 0.0], [0.04217979866481367, -0.034827086559237865, 0.23494165172785297]]\n",
      "sigma = \n",
      "[[-1.36999368  9.8788354   4.147328  ]\n",
      " [-0.58935751  9.8788354   4.147328  ]\n",
      " [-1.09039954 10.29513502  4.147328  ]\n",
      " [-1.28563408  9.80918123  4.6172113 ]\n",
      " [-2.15062984  9.8788354   4.147328  ]\n",
      " [-1.64958782  9.46253578  4.147328  ]\n",
      " [-1.45435328  9.94848958  3.67744469]]\n",
      "sqP = \n",
      "[[0.6112859897772204, 0.0, 0.0], [0.07059975520441039, 0.49627914374370335, 0.0], [0.0213601192564715, -0.015969104555127, 0.5050500793131729]]\n",
      "m = \n",
      "[-1.32801701  9.83466583  3.98390136]\n",
      "P = \n",
      "[[ 0.15230448  0.05844448  0.01642084]\n",
      " [ 0.05844448  0.05977889 -0.00255748]\n",
      " [ 0.01642084 -0.00255748  0.0594781 ]]\n",
      "num = \n",
      "1002\n",
      "SENSOR 4 2.27760317493 -0.454019833416\n",
      "\n",
      "num = \n",
      "1003\n",
      "SENSOR 5 0.989955134561 3.00334167163\n",
      "\n",
      "num = \n",
      "1004\n",
      "SENSOR 6 4.0746975399 -2.97885744051\n",
      "\n",
      "num = \n",
      "1005\n",
      "ODOMETRY -0.000250538104947 0.100118558009 0.000470538017319\n",
      "\n",
      "sqP = \n",
      "[[0.39026207094050935, 0.0, 0.0], [0.14975700556395696, 0.1932659575533099, 0.0], [0.04207644312367531, -0.045836970945003226, 0.23581061758358807]]\n",
      "sigma = \n",
      "[[-1.32801701  9.83466583  3.98390136]\n",
      " [-0.54749287  9.83466583  3.98390136]\n",
      " [-1.028503   10.22119774  3.98390136]\n",
      " [-1.24386412  9.74299188  4.4555226 ]\n",
      " [-2.10854115  9.83466583  3.98390136]\n",
      " [-1.62753102  9.44813391  3.98390136]\n",
      " [-1.41216989  9.92633977  3.51228013]]\n",
      "sqP = \n",
      "[[0.6135976465047743, 0.0, 0.0], [0.06949830385694249, 0.49109359432281546, 0.0], [0.02245205332725978, -0.02130725337696058, 0.5053133404763948]]\n",
      "m = \n",
      "[-1.36625383 10.14837633  3.92100831]\n",
      "P = \n",
      "[[ 0.2342117   0.05108487  0.01008216]\n",
      " [ 0.05108487  0.1294802  -0.01223858]\n",
      " [ 0.01008216 -0.01223858  0.06088436]]\n",
      "num = \n",
      "1006\n",
      "SENSOR 8 5.08620163194 -1.49046025052\n",
      "\n",
      "num = \n",
      "1007\n",
      "SENSOR 4 2.30195764953 -0.428336448448\n",
      "\n",
      "num = \n",
      "1008\n",
      "SENSOR 5 1.20549687351 2.9880742534\n",
      "\n",
      "num = \n",
      "1009\n",
      "SENSOR 6 4.35196037607 -2.99771407728\n",
      "\n",
      "num = \n",
      "1010\n",
      "ODOMETRY -0.000419971039834 0.0999850157042 -0.000136873067745\n",
      "\n",
      "sqP = \n",
      "[[0.4839542302942637, 0.0, 0.0], [0.10555723003200063, 0.34400272106162755, 0.0], [0.02083288620482938, -0.04196955587450036, 0.2422579424163362]]\n",
      "sigma = \n",
      "[[-1.36625383 10.14837633  3.92100831]\n",
      " [-0.39834537 10.14837633  3.92100831]\n",
      " [-1.15513937 10.83638177  3.92100831]\n",
      " [-1.32458806 10.06443722  4.40552419]\n",
      " [-2.33416229 10.14837633  3.92100831]\n",
      " [-1.57736829  9.46037089  3.92100831]\n",
      " [-1.4079196  10.23231544  3.43649243]]\n",
      "sqP = \n",
      "[[0.6676735991331134, 0.0, 0.0], [0.08403024093550386, 0.5671753173056976, 0.0], [0.013972094273586562, -0.01734872936181698, 0.5087229722645445]]\n",
      "m = \n",
      "[-1.42385098 10.05965094  3.76553538]\n",
      "P = \n",
      "[[ 0.17805128  0.04705663  0.01900074]\n",
      " [ 0.04705663  0.07803975 -0.01213897]\n",
      " [ 0.01900074 -0.01213897  0.05528531]]\n",
      "num = \n",
      "1011\n",
      "SENSOR 8 4.9606373049 -1.48374082501\n",
      "\n",
      "num = \n",
      "1012\n",
      "SENSOR 4 2.02122842241 -0.612520417005\n",
      "\n",
      "num = \n",
      "1013\n",
      "SENSOR 5 1.18548266277 2.91183826645\n",
      "\n",
      "num = \n",
      "1014\n",
      "SENSOR 6 4.40601584512 -3.00750175779\n",
      "\n",
      "num = \n",
      "1015\n",
      "ODOMETRY 0.000258908008153 0.0999880869982 -0.000878618195419\n",
      "\n",
      "sqP = \n",
      "[[0.42196123088842313, 0.0, 0.0], [0.11151884330773167, 0.2561314137541844, 0.0], [0.04502959855597212, -0.06699925618964518, 0.2208364690401133]]\n",
      "sigma = \n",
      "[[-1.42385098 10.05965094  3.76553538]\n",
      " [-0.57992851 10.05965094  3.76553538]\n",
      " [-1.20081329 10.57191377  3.76553538]\n",
      " [-1.33379178  9.92565243  4.20720832]\n",
      " [-2.26777344 10.05965094  3.76553538]\n",
      " [-1.64688866  9.54738812  3.76553538]\n",
      " [-1.51391017 10.19364946  3.32386244]]\n",
      "sqP = \n",
      "[[0.6265104927546542, 0.0, 0.0], [0.06793365672858319, 0.5215310360468637, 0.0], [0.019880349411994812, -0.027997035344582755, 0.4981044663379117]]\n",
      "m = \n",
      "[-1.52510559 10.00646207  3.73071134]\n",
      "P = \n",
      "[[ 0.15368055  0.05788313  0.00731399]\n",
      " [ 0.05788313  0.05719993 -0.00397184]\n",
      " [ 0.00731399 -0.00397184  0.06122015]]\n",
      "num = \n",
      "1016\n",
      "SENSOR 8 5.07928480298 -1.47517883113\n",
      "\n",
      "num = \n",
      "1017\n",
      "SENSOR 4 1.95540196161 -0.524791774647\n",
      "\n",
      "num = \n",
      "1018\n",
      "SENSOR 5 1.50601099601 3.02951420464\n",
      "\n",
      "num = \n",
      "1019\n",
      "SENSOR 6 4.2697515621 -2.91223271519\n",
      "\n",
      "num = \n",
      "1020\n",
      "ODOMETRY 8.35160305286e-05 0.0998921936279 -0.000916685935868\n",
      "\n",
      "sqP = \n",
      "[[0.3920211152741057, 0.0, 0.0], [0.14765308740865957, 0.1881448708882802, 0.0], [0.01865714482437883, -0.03575237102103058, 0.24411847696942038]]\n",
      "sigma = \n",
      "[[-1.52510559 10.00646207  3.73071134]\n",
      " [-0.74106336 10.00646207  3.73071134]\n",
      " [-1.22979941 10.38275181  3.73071134]\n",
      " [-1.4877913   9.93495733  4.2189483 ]\n",
      " [-2.30914782 10.00646207  3.73071134]\n",
      " [-1.82041176  9.63017233  3.73071134]\n",
      " [-1.56241988 10.07796681  3.24247439]]\n",
      "sqP = \n",
      "[[0.6130498169766148, 0.0, 0.0], [0.070335135006985, 0.48838959163289525, 0.0], [0.008629858234303405, -0.01780749545908627, 0.5093074113256347]]\n",
      "m = \n",
      "[-1.5014142   9.88836111  3.67878415]\n",
      "P = \n",
      "[[ 0.14179143  0.03100839  0.00961645]\n",
      " [ 0.03100839  0.08158369 -0.00805009]\n",
      " [ 0.00961645 -0.00805009  0.06119866]]\n",
      "num = \n",
      "1021\n",
      "SENSOR 8 5.12480337929 -1.5292251446\n",
      "\n",
      "num = \n",
      "1022\n",
      "SENSOR 4 1.8502364216 -0.640053414064\n",
      "\n",
      "num = \n",
      "1023\n",
      "SENSOR 5 1.22548414583 3.00267777115\n",
      "\n",
      "num = \n",
      "1024\n",
      "SENSOR 6 4.59119051094 -3.00286235802\n",
      "\n",
      "num = \n",
      "1025\n",
      "ODOMETRY 0.000276055660063 0.0999750533485 0.000328587305689\n",
      "\n",
      "sqP = \n",
      "[[0.3765520261566873, 0.0, 0.0], [0.08234822082702184, 0.2735003871419688, 0.0], [0.025538159602300825, -0.0371228584243233, 0.2432454654480693]]\n",
      "sigma = \n",
      "[[-1.5014142   9.88836111  3.67878415]\n",
      " [-0.74831015  9.88836111  3.67878415]\n",
      " [-1.33671776 10.43536188  3.67878415]\n",
      " [-1.45033788  9.81411539  4.16527508]\n",
      " [-2.25451825  9.88836111  3.67878415]\n",
      " [-1.66611064  9.34136033  3.67878415]\n",
      " [-1.55249052  9.96260682  3.19229321]]\n",
      "sqP = \n",
      "[[0.5909524984903228, 0.0, 0.0], [0.059468928120323325, 0.5274074799178324, 0.0], [0.012109511710646061, -0.016885843241273294, 0.5088812986376228]]\n",
      "m = \n",
      "[-1.44568456  9.88127117  3.62544518]\n",
      "P = \n",
      "[[ 0.13128644  0.02462105  0.01008577]\n",
      " [ 0.02462105  0.08836081 -0.01074661]\n",
      " [ 0.01008577 -0.01074661  0.06109553]]\n",
      "num = \n",
      "1026\n",
      "SENSOR 8 4.85707120873 -1.55643745133\n",
      "\n",
      "num = \n",
      "1027\n",
      "SENSOR 4 1.6853612932 -0.640165672521\n",
      "\n",
      "num = \n",
      "1028\n",
      "SENSOR 5 1.40741969379 3.08301703619\n",
      "\n",
      "num = \n",
      "1029\n",
      "SENSOR 6 4.43722471141 -2.92421694343\n",
      "\n",
      "num = \n",
      "1030\n",
      "ODOMETRY -0.000612253935663 0.100030803164 0.000384082346755\n",
      "\n",
      "sqP = \n",
      "[[0.3623347049746025, 0.0, 0.0], [0.06795111255598899, 0.2893846082057733, 0.0], [0.027835514900518842, -0.04367220927896204, 0.2416887414841371]]\n",
      "sigma = \n",
      "[[-1.44568456  9.88127117  3.62544518]\n",
      " [-0.72101515  9.88127117  3.62544518]\n",
      " [-1.30978233 10.46004039  3.62544518]\n",
      " [-1.39001353  9.79392675  4.10882266]\n",
      " [-2.17035397  9.88127117  3.62544518]\n",
      " [-1.58158678  9.30250196  3.62544518]\n",
      " [-1.50135559  9.96861559  3.1420677 ]]\n",
      "sqP = \n",
      "[[0.5802401296156668, 0.0, 0.0], [0.053243267258774575, 0.5362827468348655, 0.0], [0.014298004981474769, -0.019189275340503957, 0.5080750285044411]]\n",
      "m = \n",
      "[-1.47450771  9.80845912  3.5583158 ]\n",
      "P = \n",
      "[[ 0.13323808  0.05256706  0.00407208]\n",
      " [ 0.05256706  0.06106096 -0.00558867]\n",
      " [ 0.00407208 -0.00558867  0.06045447]]\n",
      "num = \n",
      "1031\n",
      "SENSOR 8 4.8148264003 -1.5861711641\n",
      "\n",
      "num = \n",
      "1032\n",
      "SENSOR 4 1.62945141541 -0.645906242858\n",
      "\n",
      "num = \n",
      "1033\n",
      "SENSOR 5 1.57411900279 3.00136290051\n",
      "\n",
      "num = \n",
      "1034\n",
      "SENSOR 6 4.74165161304 -2.88381335457\n",
      "\n",
      "num = \n",
      "1035\n",
      "ODOMETRY -0.000615965348572 0.0999645830188 0.000122066793836\n",
      "\n",
      "sqP = \n",
      "[[0.36501791317313753, 0.0, 0.0], [0.14401228119727402, 0.20080195639940104, 0.0], [0.01115582425934357, -0.0358325544738924, 0.24299392073647902]]\n",
      "sigma = \n",
      "[[-1.47450771  9.80845912  3.5583158 ]\n",
      " [-0.74447188  9.80845912  3.5583158 ]\n",
      " [-1.18648315 10.21006304  3.5583158 ]\n",
      " [-1.45219606  9.73679402  4.04430364]\n",
      " [-2.20454354  9.80845912  3.5583158 ]\n",
      " [-1.76253227  9.40685521  3.5583158 ]\n",
      " [-1.49681936  9.88012423  3.07232796]]\n",
      "sqP = \n",
      "[[0.5950647582304133, 0.0, 0.0], [0.07173469398972868, 0.4933195877248994, 0.0], [0.006840511707025627, -0.01732082393179547, 0.508796468781268]]\n",
      "m = \n",
      "[-1.42149907  9.69615625  3.41547903]\n",
      "P = \n",
      "[[ 0.13405103  0.03070211  0.00818374]\n",
      " [ 0.03070211  0.08229374 -0.00960711]\n",
      " [ 0.00818374 -0.00960711  0.06084606]]\n",
      "num = \n",
      "1036\n",
      "SENSOR 8 5.09246076865 -1.62935789257\n",
      "\n",
      "num = \n",
      "1037\n",
      "SENSOR 4 1.50283498391 -0.764000413422\n",
      "\n",
      "num = \n",
      "1038\n",
      "SENSOR 5 1.65903158666 3.20124071317\n",
      "\n",
      "num = \n",
      "1039\n",
      "SENSOR 6 4.68619938405 -3.0067714176\n",
      "\n",
      "num = \n",
      "1040\n",
      "ODOMETRY 0.00010181891152 0.100046833019 0.000440816980011\n",
      "\n",
      "sqP = \n",
      "[[0.36612979868664497, 0.0, 0.0], [0.08385579497651281, 0.2743391009970222, 0.0], [0.022352014862581406, -0.04185131489129899, 0.2420638619568476]]\n",
      "sigma = \n",
      "[[-1.42149907  9.69615625  3.41547903]\n",
      " [-0.68923947  9.69615625  3.41547903]\n",
      " [-1.25378748 10.24483445  3.41547903]\n",
      " [-1.37679504  9.61245362  3.89960676]\n",
      " [-2.15375867  9.69615625  3.41547903]\n",
      " [-1.58921066  9.14747805  3.41547903]\n",
      " [-1.4662031   9.77985888  2.93135131]]\n",
      "sqP = \n",
      "[[0.5844505423095124, 0.0, 0.0], [0.05973179532705194, 0.5281625529337269, 0.0], [0.01254853521441494, -0.018674488097121654, 0.5083218378990995]]\n",
      "m = \n",
      "[-1.29459536  9.75172307  3.33625104]\n",
      "P = \n",
      "[[ 0.13604164  0.05350699  0.00434851]\n",
      " [ 0.05350699  0.05942154 -0.00567771]\n",
      " [ 0.00434851 -0.00567771  0.06036408]]\n",
      "num = \n",
      "1041\n",
      "SENSOR 8 5.10307136294 -1.5981894044\n",
      "\n",
      "num = \n",
      "1042\n",
      "SENSOR 4 1.3396692961 -0.647480763035\n",
      "\n",
      "num = \n",
      "1043\n",
      "SENSOR 5 1.89463338292 3.13168562413\n",
      "\n",
      "num = \n",
      "1044\n",
      "SENSOR 6 4.79308799565 -3.00728766374\n",
      "\n",
      "num = \n",
      "1045\n",
      "ODOMETRY -0.000291816369108 0.100048609814 -0.00104644833934\n",
      "\n",
      "sqP = \n",
      "[[0.3688382358681174, 0.0, 0.0], [0.1450690015172853, 0.19589927410016691, 0.0], [0.011789751836137808, -0.03771344485107456, 0.2424928380161651]]\n",
      "sigma = \n",
      "[[-1.29459536  9.75172307  3.33625104]\n",
      " [-0.55691888  9.75172307  3.33625104]\n",
      " [-1.00445735 10.14352162  3.33625104]\n",
      " [-1.27101585  9.67629618  3.82123671]\n",
      " [-2.03227183  9.75172307  3.33625104]\n",
      " [-1.58473336  9.35992452  3.33625104]\n",
      " [-1.31817486  9.82714996  2.85126536]]\n",
      "sqP = \n",
      "[[0.5976835765586043, 0.0, 0.0], [0.07014346639779646, 0.49136002708116516, 0.0], [0.011401247012448053, -0.017495329482169135, 0.5086551672373721]]\n",
      "m = \n",
      "[-1.6055043   9.98276317  3.1441269 ]\n",
      "P = \n",
      "[[ 0.16544186  0.00710811  0.0096299 ]\n",
      " [ 0.00710811  0.09986483 -0.00799205]\n",
      " [ 0.0096299  -0.00799205  0.06159912]]\n",
      "num = \n",
      "1046\n",
      "SENSOR 8 5.05308894221 -1.61739005656\n",
      "\n",
      "num = \n",
      "1047\n",
      "SENSOR 4 1.55728242344 -0.778699498829\n",
      "\n",
      "num = \n",
      "1048\n",
      "SENSOR 5 1.76096780816 3.0522097073\n",
      "\n",
      "num = \n",
      "1049\n",
      "ODOMETRY 8.52183257962e-05 0.0998196908854 0.000109532441973\n",
      "\n",
      "sqP = \n",
      "[[0.4067454517635404, 0.0, 0.0], [0.01747558223718215, 0.3155304081033354, 0.0], [0.02367550576432339, -0.02664018547143545, 0.24561940403295002]]\n",
      "sigma = \n",
      "[[-1.6055043   9.98276317  3.1441269 ]\n",
      " [-0.7920134   9.98276317  3.1441269 ]\n",
      " [-1.57055314 10.61382398  3.1441269 ]\n",
      " [-1.55815329  9.9294828   3.6353657 ]\n",
      " [-2.41899521  9.98276317  3.1441269 ]\n",
      " [-1.64045547  9.35170235  3.1441269 ]\n",
      " [-1.65285532 10.03604354  2.65288809]]\n",
      "sqP = \n",
      "[[0.6052336640323732, 0.0, 0.0], [0.03262694887813064, 0.5490990140317404, 0.0], [0.008748239775822769, -0.011481993758012524, 0.5101612104176689]]\n",
      "m = \n",
      "[-1.28773729  9.76307752  3.3843487 ]\n",
      "P = \n",
      "[[ 0.18234756  0.0191126  -0.00837172]\n",
      " [ 0.0191126   0.08011703  0.00653434]\n",
      " [-0.00837172  0.00653434  0.07194349]]\n",
      "num = \n",
      "1050\n",
      "SENSOR 8 4.93860295592 -1.59049668067\n",
      "\n",
      "num = \n",
      "1051\n",
      "SENSOR 4 1.44172934018 -0.934292188086\n",
      "\n",
      "num = \n",
      "1052\n",
      "SENSOR 5 1.92234124612 3.05518288622\n",
      "\n",
      "num = \n",
      "1053\n",
      "ODOMETRY 7.22338246953e-05 0.0999023454001 -3.77129042551e-05\n",
      "\n",
      "sqP = \n",
      "[[0.42702173758745043, 0.0, 0.0], [0.044757918782312, 0.279488391070567, 0.0], [-0.01960489832122727, 0.026519215847164244, 0.26618766213066236]]\n",
      "sigma = \n",
      "[[-1.28773729  9.76307752  3.3843487 ]\n",
      " [-0.43369381  9.76307752  3.3843487 ]\n",
      " [-1.19822145 10.3220543   3.3843487 ]\n",
      " [-1.32694708  9.81611595  3.91672402]\n",
      " [-2.14178076  9.76307752  3.3843487 ]\n",
      " [-1.37725312  9.20410074  3.3843487 ]\n",
      " [-1.24852749  9.71003909  2.85197338]]\n",
      "sqP = \n",
      "[[0.6202702537637245, 0.0, 0.0], [0.04457193356886723, 0.5295939699005753, 0.0], [-0.0010687722461389707, 0.013724335356681647, 0.52027743529629]]\n",
      "m = \n",
      "[-1.18822876  9.69308932  3.20112277]\n",
      "P = \n",
      "[[ 0.19561612  0.01656274  0.01040071]\n",
      " [ 0.01656274  0.08741443 -0.01614977]\n",
      " [ 0.01040071 -0.01614977  0.08769363]]\n",
      "num = \n",
      "1054\n",
      "SENSOR 8 5.1479288931 -1.65822977182\n",
      "\n",
      "num = \n",
      "1055\n",
      "SENSOR 4 1.25455156912 -0.879623084747\n",
      "\n",
      "num = \n",
      "1056\n",
      "SENSOR 5 2.37861473612 3.10537197028\n",
      "\n",
      "num = \n",
      "1057\n",
      "ODOMETRY 0.000670127696266 0.100006079133 0.000625609916127\n",
      "\n",
      "sqP = \n",
      "[[0.4422851165254464, 0.0, 0.0], [0.03744810450391912, 0.293278136402784, 0.0], [0.02351585866627175, -0.058069078818600094, 0.28942808812515836]]\n",
      "sigma = \n",
      "[[-1.18822876  9.69308932  3.20112277]\n",
      " [-0.30365852  9.69308932  3.20112277]\n",
      " [-1.11333255 10.27964559  3.20112277]\n",
      " [-1.14119704  9.57695116  3.77997895]\n",
      " [-2.07279899  9.69308932  3.20112277]\n",
      " [-1.26312497  9.10653305  3.20112277]\n",
      " [-1.23526047  9.80922748  2.62226659]]\n",
      "sqP = \n",
      "[[0.6305326957737213, 0.0, 0.0], [0.0406862760475154, 0.5393016159214206, 0.0], [0.021030814419481673, -0.030105528456245003, 0.5323092087275172]]\n",
      "m = \n",
      "[-1.09657933  9.90566324  2.84685781]\n",
      "P = \n",
      "[[ 0.12561725 -0.00514248  0.01366721]\n",
      " [-0.00514248  0.10958135 -0.02058478]\n",
      " [ 0.01366721 -0.02058478  0.09514714]]\n",
      "num = \n",
      "1058\n",
      "SENSOR 8 5.09480518246 -1.65212699893\n",
      "\n",
      "num = \n",
      "1059\n",
      "SENSOR 4 1.40390131939 -0.879816536972\n",
      "\n",
      "num = \n",
      "1060\n",
      "SENSOR 5 2.20542921923 3.11477051951\n",
      "\n",
      "num = \n",
      "1061\n",
      "ODOMETRY 0.000570783669274 0.0999546481828 0.000310239449538\n",
      "\n",
      "sqP = \n",
      "[[0.3544252342357192, 0.0, 0.0], [-0.01450935893399351, 0.33071261271402996, 0.0], [0.038561604042681275, -0.06055188829876524, 0.2999893535550881]]\n",
      "sigma = \n",
      "[[-1.09657933  9.90566324  2.84685781]\n",
      " [-0.38772886  9.90566324  2.84685781]\n",
      " [-1.12559805 10.56708846  2.84685781]\n",
      " [-1.01945612  9.78455946  3.44683651]\n",
      " [-1.8054298   9.90566324  2.84685781]\n",
      " [-1.06756061  9.24423801  2.84685781]\n",
      " [-1.17370254 10.02676701  2.2468791 ]]\n",
      "sqP = \n",
      "[[0.5721142940585076, 0.0, 0.0], [0.005728598947262158, 0.5591154908176174, 0.0], [0.02957552191320023, -0.030832025907135397, 0.5380850258695888]]\n",
      "m = \n",
      "[-0.97401839  9.97323408  2.8476893 ]\n",
      "P = \n",
      "[[ 0.10094193 -0.01210819  0.01350909]\n",
      " [-0.01210819  0.09963016 -0.01249179]\n",
      " [ 0.01350909 -0.01249179  0.10466798]]\n",
      "num = \n",
      "1062\n",
      "SENSOR 4 1.06631403361 -1.08193342846\n",
      "\n",
      "num = \n",
      "1063\n",
      "SENSOR 5 2.47203994196 3.13761545266\n",
      "\n",
      "num = \n",
      "1064\n",
      "ODOMETRY 3.85114033074e-05 0.100176546577 0.000173947982474\n",
      "\n",
      "sqP = \n",
      "[[0.3177135946936443, 0.0, 0.0], [-0.03811038725582995, 0.31333330250558694, 0.0], [0.042519698491311424, -0.03469578490423701, 0.3188357820597589]]\n",
      "sigma = \n",
      "[[-0.97401839  9.97323408  2.8476893 ]\n",
      " [-0.3385912   9.97323408  2.8476893 ]\n",
      " [-1.05023916 10.59990068  2.8476893 ]\n",
      " [-0.88897899  9.90384251  3.48536086]\n",
      " [-1.60944558  9.97323408  2.8476893 ]\n",
      " [-0.89779761  9.34656747  2.8476893 ]\n",
      " [-1.05905779 10.04262565  2.21001774]]\n",
      "sqP = \n",
      "[[0.5515453332498959, 0.0, 0.0], [-0.009709323346313508, 0.5456351214525105, 0.0], [0.034522213194519265, -0.018271727002999387, 0.5495356693096184]]\n",
      "m = \n",
      "[-0.85363952  9.82075758  2.65217534]\n",
      "P = \n",
      "[[ 0.18879036 -0.02780085  0.02369733]\n",
      " [-0.02780085  0.18150641 -0.02564727]\n",
      " [ 0.02369733 -0.02564727  0.11015778]]\n",
      "num = \n",
      "1065\n",
      "SENSOR 4 1.20265396615 -1.09036457216\n",
      "\n",
      "num = \n",
      "1066\n",
      "SENSOR 5 2.34144827576 3.04411849412\n",
      "\n",
      "num = \n",
      "1067\n",
      "ODOMETRY 0.000776427377761 0.099952687123 -0.000941767872499\n",
      "\n",
      "sqP = \n",
      "[[0.43450012176421643, 0.0, 0.0], [-0.06398352930118456, 0.42120365122305675, 0.0], [0.05453928614933888, -0.052605554082417634, 0.32313449080526735]]\n",
      "sigma = \n",
      "[[-0.85363952  9.82075758  2.65217534]\n",
      " [ 0.01536072  9.82075758  2.65217534]\n",
      " [-0.98160658 10.66316489  2.65217534]\n",
      " [-0.74456095  9.71554647  3.29844432]\n",
      " [-1.72263976  9.82075758  2.65217534]\n",
      " [-0.72567246  8.97835028  2.65217534]\n",
      " [-0.96271809  9.92596869  2.00590636]]\n",
      "sqP = \n",
      "[[0.6291730934875028, 0.0, 0.0], [-0.026639495689784076, 0.613147765746584, 0.0], [0.04611117648497586, -0.025694993441313083, 0.5520316720377922]]\n",
      "m = \n",
      "[-0.71554268  9.75130315  2.7069792 ]\n",
      "P = \n",
      "[[ 0.24978807 -0.05249958  0.03924509]\n",
      " [-0.05249958  0.22283783 -0.03012481]\n",
      " [ 0.03924509 -0.03012481  0.11505609]]\n",
      "num = \n",
      "1068\n",
      "SENSOR 4 1.16717333388 -1.12821635833\n",
      "\n",
      "num = \n",
      "1069\n",
      "SENSOR 5 2.46018754147 3.12942300941\n",
      "\n",
      "num = \n",
      "1070\n",
      "ODOMETRY -0.000182525437042 0.0999468132926 -0.000158790994297\n",
      "\n",
      "sqP = \n",
      "[[0.49978802520909393, 0.0, 0.0], [-0.10504369122042748, 0.46022131339356565, 0.0], [0.07852346418961374, -0.047534550898291755, 0.3265434509213424]]\n",
      "sigma = \n",
      "[[-0.71554268  9.75130315  2.7069792 ]\n",
      " [ 0.28403337  9.75130315  2.7069792 ]\n",
      " [-0.92563007 10.67174578  2.7069792 ]\n",
      " [-0.55849575  9.65623405  3.36006611]\n",
      " [-1.71511873  9.75130315  2.7069792 ]\n",
      " [-0.5054553   8.83086052  2.7069792 ]\n",
      " [-0.87258961  9.84637225  2.0538923 ]]\n",
      "sqP = \n",
      "[[0.6833653354070252, 0.0, 0.0], [-0.05441278410140342, 0.6366903454212879, 0.0], [0.06263281348581133, -0.021315993068731092, 0.5544145420847445]]\n",
      "m = \n",
      "[-0.58115769  9.67937947  2.67060045]\n",
      "P = \n",
      "[[ 0.3007923  -0.08293291  0.05452928]\n",
      " [-0.08293291  0.2615402  -0.0410306 ]\n",
      " [ 0.05452928 -0.0410306   0.11672613]]\n",
      "num = \n",
      "1071\n",
      "SENSOR 4 1.0123884597 -1.2839270903\n",
      "\n",
      "num = \n",
      "1072\n",
      "SENSOR 5 2.62303723473 2.94037324821\n",
      "\n",
      "num = \n",
      "1073\n",
      "ODOMETRY -0.000682345311597 0.099958072456 0.00043862067399\n",
      "\n",
      "sqP = \n",
      "[[0.5484453506096205, 0.0, 0.0], [-0.151214542572852, 0.4885430995043681, 0.0], [0.09942517762302898, -0.053211400016678234, 0.32250475053327743]]\n",
      "sigma = \n",
      "[[-0.58115769  9.67937947  2.67060045]\n",
      " [ 0.51573301  9.67937947  2.67060045]\n",
      " [-0.88358678 10.65646567  2.67060045]\n",
      " [-0.38230734  9.57295667  3.31560995]\n",
      " [-1.67804839  9.67937947  2.67060045]\n",
      " [-0.27872861  8.70229327  2.67060045]\n",
      " [-0.78000805  9.78580227  2.02559095]]\n",
      "sqP = \n",
      "[[0.7304406248533808, 0.0, 0.0], [-0.08761609724449239, 0.6529293303077237, 0.0], [0.07549873893314459, -0.022200334729129613, 0.5521741288707107]]\n",
      "m = \n",
      "[-0.21965169  9.68818443  2.49834088]\n",
      "P = \n",
      "[[ 0.21881314 -0.03613877  0.0172193 ]\n",
      " [-0.03613877  0.32026891 -0.06154809]\n",
      " [ 0.0172193  -0.06154809  0.1242722 ]]\n",
      "num = \n",
      "1074\n",
      "SENSOR 4 1.00020435353 -1.2311017102\n",
      "\n",
      "num = \n",
      "1075\n",
      "SENSOR 5 2.83365938044 3.10650495611\n",
      "\n",
      "num = \n",
      "1076\n",
      "ODOMETRY -9.19632135689e-07 0.10002313352 -0.000224880476229\n",
      "\n",
      "sqP = \n",
      "[[0.46777466734250395, 0.0, 0.0], [-0.07725679442264989, 0.5606249139966811, 0.0], [0.03681110300493328, -0.10471205216566787, 0.33459307271653005]]\n",
      "sigma = \n",
      "[[-0.21965169  9.68818443  2.49834088]\n",
      " [ 0.71589764  9.68818443  2.49834088]\n",
      " [-0.37416528 10.80943426  2.49834088]\n",
      " [-0.14602949  9.47876033  3.16752702]\n",
      " [-1.15520103  9.68818443  2.49834088]\n",
      " [-0.0651381   8.5669346   2.49834088]\n",
      " [-0.2932739   9.89760854  1.82915473]]\n",
      "sqP = \n",
      "[[0.6527915509535135, 0.0, 0.0], [-0.06570911817896258, 0.7205990188079384, 0.0], [0.04808887911137209, -0.049812256795643446, 0.5578387735845997]]\n",
      "m = \n",
      "[-0.67351093  9.84614736  2.32231553]\n",
      "P = \n",
      "[[ 0.25404106  0.02496308  0.03651184]\n",
      " [ 0.02496308  0.35503464 -0.07255481]\n",
      " [ 0.03651184 -0.07255481  0.11115001]]\n",
      "num = \n",
      "1077\n",
      "SENSOR 4 0.849352538956 -1.46997822321\n",
      "\n",
      "num = \n",
      "1078\n",
      "SENSOR 5 2.85184515953 3.11408226771\n",
      "\n",
      "num = \n",
      "1079\n",
      "ODOMETRY -0.000798214396223 0.0999079224924 0.000213586041509\n",
      "\n",
      "sqP = \n",
      "[[0.5040248636375552, 0.0, 0.0], [0.04952748138024997, 0.5937858773612463, 0.0], [0.07244054640961539, -0.12823244041982926, 0.29909667240622245]]\n",
      "sigma = \n",
      "[[-0.67351093  9.84614736  2.32231553]\n",
      " [ 0.3345388   9.84614736  2.32231553]\n",
      " [-0.57445596 11.03371911  2.32231553]\n",
      " [-0.52862983  9.58968248  2.92050887]\n",
      " [-1.68156065  9.84614736  2.32231553]\n",
      " [-0.77256589  8.6585756   2.32231553]\n",
      " [-0.81839202 10.10261224  1.72412218]]\n",
      "sqP = \n",
      "[[0.6795157594499563, 0.0, 0.0], [0.04988335469214426, 0.7549935646462274, 0.0], [0.057292973134735095, -0.04977282655348877, 0.5370012848255408]]\n",
      "m = \n",
      "[-0.41306355  9.6438891   2.22663188]\n",
      "P = \n",
      "[[ 0.20477733  0.08014975 -0.00651251]\n",
      " [ 0.08014975  0.31794122 -0.03944459]\n",
      " [-0.00651251 -0.03944459  0.12775011]]\n",
      "num = \n",
      "1080\n",
      "SENSOR 4 1.10312028502 -1.48363601494\n",
      "\n",
      "num = \n",
      "1081\n",
      "SENSOR 5 3.00416166275 3.10884116279\n",
      "\n",
      "num = \n",
      "1082\n",
      "ODOMETRY -0.000984233781833 0.0999672948533 7.64590930276e-05\n",
      "\n",
      "sqP = \n",
      "[[0.45252329286607595, 0.0, 0.0], [0.17711740520746955, 0.5353229352437329, 0.0], [-0.014391545698911523, -0.06892212105247986, 0.3504179413280621]]\n",
      "sigma = \n",
      "[[-0.41306355  9.6438891   2.22663188]\n",
      " [ 0.49198303  9.6438891   2.22663188]\n",
      " [-0.05882874 10.71453497  2.22663188]\n",
      " [-0.44184664  9.50604485  2.92746776]\n",
      " [-1.31811014  9.6438891   2.22663188]\n",
      " [-0.76729836  8.57324323  2.22663188]\n",
      " [-0.38428046  9.78173334  1.525796  ]]\n",
      "sqP = \n",
      "[[0.6605717390271757, 0.0, 0.0], [0.15774038348174257, 0.6888309089219637, 0.0], [0.021289055199882903, -0.026917484951604818, 0.5676973748845566]]\n",
      "m = \n",
      "[-0.18582326  9.76125432  2.1849119 ]\n",
      "P = \n",
      "[[ 0.271428    0.13141853  0.02511041]\n",
      " [ 0.13141853  0.32571546 -0.06118251]\n",
      " [ 0.02511041 -0.06118251  0.1206743 ]]\n",
      "num = \n",
      "1083\n",
      "SENSOR 4 1.04848307593 -1.64994919996\n",
      "\n",
      "num = \n",
      "1084\n",
      "SENSOR 5 3.02744738541 3.06646084819\n",
      "\n",
      "num = \n",
      "1085\n",
      "ODOMETRY -0.000260628905805 0.0999789088987 0.00116057007669\n",
      "\n",
      "sqP = \n",
      "[[0.5209875209544379, 0.0, 0.0], [0.25224890010480006, 0.51194330732341, 0.0], [0.048197712172325186, -0.1432587011543582, 0.31277503780514027]]\n",
      "sigma = \n",
      "[[-0.18582326  9.76125432  2.1849119 ]\n",
      " [ 0.85615178  9.76125432  2.1849119 ]\n",
      " [ 0.31867454 10.78514094  2.1849119 ]\n",
      " [-0.08942784  9.47473692  2.81046197]\n",
      " [-1.22779831  9.76125432  2.1849119 ]\n",
      " [-0.69032106  8.73736771  2.1849119 ]\n",
      " [-0.28221869 10.04777173  1.55936182]]\n",
      "sqP = \n",
      "[[0.7330624284827171, 0.0, 0.0], [0.17404122366401312, 0.6761187215983856, 0.0], [0.0591816833890588, -0.06356255224631231, 0.5437285919731603]]\n",
      "m = \n",
      "[-0.06721192  9.66667803  2.18984131]\n",
      "P = \n",
      "[[ 0.33263911  0.07340686  0.05052836]\n",
      " [ 0.07340686  0.29828874 -0.02243873]\n",
      " [ 0.05052836 -0.02243873  0.12306203]]\n",
      "num = \n",
      "1086\n",
      "SENSOR 4 1.01403868185 -1.8074118964\n",
      "\n",
      "num = \n",
      "1087\n",
      "SENSOR 5 3.08384570669 3.04030179903\n",
      "\n",
      "num = \n",
      "1088\n",
      "ODOMETRY -0.000220095556118 0.0999551962105 -0.000481994451249\n",
      "\n",
      "sqP = \n",
      "[[0.5767487421839139, 0.0, 0.0], [0.12727701629068283, 0.5311207953135018, 0.0], [0.08760896929175283, -0.0632423719609495, 0.33374706626790734]]\n",
      "sigma = \n",
      "[[-0.06721192  9.66667803  2.18984131]\n",
      " [ 1.08628556  9.66667803  2.18984131]\n",
      " [ 0.18734211 10.72891962  2.18984131]\n",
      " [ 0.10800601  9.54019328  2.85733544]\n",
      " [-1.22070941  9.66667803  2.18984131]\n",
      " [-0.32176596  8.60443644  2.18984131]\n",
      " [-0.24242986  9.79316277  1.52234717]]\n",
      "sqP = \n",
      "[[0.7459985804188564, 0.0, 0.0], [0.0857848901655273, 0.6929603687111063, 0.0], [0.07760973147912449, -0.03078674511150277, 0.559048013146258]]\n",
      "m = \n",
      "[-0.73724867  9.69408536  2.1263061 ]\n",
      "P = \n",
      "[[ 0.3121703   0.03876094  0.04326699]\n",
      " [ 0.03876094  0.31385557 -0.01854287]\n",
      " [ 0.04326699 -0.01854287  0.12212328]]\n",
      "num = \n",
      "1089\n",
      "SENSOR 4 0.949362251022 -1.93475998957\n",
      "\n",
      "num = \n",
      "1090\n",
      "SENSOR 5 3.02874489038 3.02677668221\n",
      "\n",
      "num = \n",
      "1091\n",
      "ODOMETRY 0.00031060131166 0.100181543701 -0.20019265501\n",
      "\n",
      "sqP = \n",
      "[[0.558722019556902, 0.0, 0.0], [0.06937428004292438, 0.5559161628250481, 0.0], [0.07743920869886214, -0.043019369927433396, 0.3380470075259276]]\n",
      "sigma = \n",
      "[[-0.73724867  9.69408536  2.1263061 ]\n",
      " [ 0.38019536  9.69408536  2.1263061 ]\n",
      " [-0.59850011 10.80591769  2.1263061 ]\n",
      " [-0.58237026  9.60804662  2.80240012]\n",
      " [-1.85469271  9.69408536  2.1263061 ]\n",
      " [-0.87599723  8.58225303  2.1263061 ]\n",
      " [-0.89212709  9.7801241   1.45021209]]\n",
      "sqP = \n",
      "[[0.7231735040176205, 0.0, 0.0], [0.07306456258264708, 0.715229887446133, 0.0], [0.06299369869543806, -0.01960648320962561, 0.5614327177694012]]\n",
      "m = \n",
      "[-0.64043905  9.77728898  2.15334364]\n",
      "P = \n",
      "[[ 0.33255602  0.09757775  0.05474763]\n",
      " [ 0.09757775  0.31506732 -0.0539026 ]\n",
      " [ 0.05474763 -0.0539026   0.12079177]]\n",
      "num = \n",
      "1092\n",
      "SENSOR 4 1.03535256961 -1.66182916184\n",
      "\n",
      "num = \n",
      "1093\n",
      "SENSOR 5 3.32942750619 -2.89908305281\n",
      "\n",
      "num = \n",
      "1094\n",
      "ODOMETRY -6.00078729292e-05 0.100231170262 -0.197349468283\n",
      "\n",
      "sqP = \n",
      "[[0.5766767074927464, 0.0, 0.0], [0.16920702748023023, 0.5351974424752703, 0.0], [0.0949364422071421, -0.13073028372450515, 0.30771486735682063]]\n",
      "sigma = \n",
      "[[-0.64043905  9.77728898  2.15334364]\n",
      " [ 0.51291437  9.77728898  2.15334364]\n",
      " [-0.30202499 10.84768386  2.15334364]\n",
      " [-0.45056616  9.51582841  2.76877337]\n",
      " [-1.79379246  9.77728898  2.15334364]\n",
      " [-0.9788531   8.70689409  2.15334364]\n",
      " [-0.83031193 10.03874955  1.5379139 ]]\n",
      "sqP = \n",
      "[[0.7551158660578652, 0.0, 0.0], [0.12731420444946756, 0.7055123214976273, 0.0], [0.0706749986102758, -0.057342701018366846, 0.5411792614038592]]\n",
      "m = \n",
      "[-2.00179228 10.06108868  1.83811835]\n",
      "P = \n",
      "[[ 0.31022478  0.14501559  0.00174082]\n",
      " [ 0.14501559  0.28532291 -0.01989787]\n",
      " [ 0.00174082 -0.01989787  0.13080452]]\n",
      "num = \n",
      "1095\n",
      "SENSOR 4 1.23205472755 -1.61328765107\n",
      "\n",
      "num = \n",
      "1096\n",
      "SENSOR 5 3.58883871764 -2.79036998463\n",
      "\n",
      "num = \n",
      "1097\n",
      "ODOMETRY 0.000778944546602 0.0997443640523 -0.196805293229\n",
      "\n",
      "sqP = \n",
      "[[0.5569782562328962, 0.0, 0.0], [0.26036130712130107, 0.46640636730094037, 0.0], [0.003125466602602048, -0.04440681220875783, 0.3589189117628575]]\n",
      "sigma = \n",
      "[[-2.00179228 10.06108868  1.83811835]\n",
      " [-0.88783577 10.06108868  1.83811835]\n",
      " [-1.48106967 10.99390142  1.83811835]\n",
      " [-1.99554135  9.97227506  2.55595617]\n",
      " [-3.11574879 10.06108868  1.83811835]\n",
      " [-2.5225149   9.12827595  1.83811835]\n",
      " [-2.00804321 10.14990231  1.12028052]]\n",
      "sqP = \n",
      "[[1.7737462288021104, 0.0, 0.0], [0.055512158034376084, 0.663178213777018, 0.0], [0.02540935374596353, -0.03453553909497661, 0.5725280431951085]]\n",
      "m = \n",
      "[-0.68495084 10.78760454  1.63180664]\n",
      "P = \n",
      "[[ 3.02370989 -0.03737978  0.03992469]\n",
      " [-0.03737978  0.10041655  0.00365787]\n",
      " [ 0.03992469  0.00365787  0.12664853]]\n",
      "num = \n",
      "1098\n",
      "SENSOR 4 0.980807526601 -1.37607808602\n",
      "\n",
      "num = \n",
      "1099\n",
      "SENSOR 5 3.44799435666 -2.62665666687\n",
      "\n",
      "num = \n",
      "1100\n",
      "ODOMETRY -0.000368902911915 0.100159655151 -0.19959086214\n",
      "\n",
      "sqP = \n",
      "[[1.7388817924085134, 0.0, 0.0], [-0.02149644396253368, 0.31615573826397386, 0.0], [0.02295997738121848, 0.013130946813183221, 0.3548928641923503]]\n",
      "sigma = \n",
      "[[-0.68495084 10.78760454  1.63180664]\n",
      " [ 2.79281274 10.78760454  1.63180664]\n",
      " [-0.72794373 11.41991602  1.63180664]\n",
      " [-0.63903089 10.81386644  2.34159237]\n",
      " [-4.16271442 10.78760454  1.63180664]\n",
      " [-0.64195795 10.15529307  1.63180664]\n",
      " [-0.73087079 10.76134265  0.92202091]]\n",
      "sqP = \n",
      "[[1.4421486770160632, 0.0, 0.0], [-0.06702768530872702, 0.5469537612492023, 0.0], [0.054257826740584464, 0.009132891829535278, 0.571982129372843]]\n",
      "m = \n",
      "[-0.34228661 10.49493889  1.85346767]\n",
      "P = \n",
      "[[ 2.05745464 -0.06153399  0.06867783]\n",
      " [-0.06153399  0.10637015 -0.00585352]\n",
      " [ 0.06867783 -0.00585352  0.10007727]]\n",
      "num = \n",
      "1101\n",
      "SENSOR 4 1.30954404554 -1.43176636885\n",
      "\n",
      "num = \n",
      "1102\n",
      "SENSOR 5 3.51128233361 -2.49583044536\n",
      "\n",
      "num = \n",
      "1103\n",
      "ODOMETRY 0.000875408598912 0.10049913959 -0.198612822628\n",
      "\n",
      "sqP = \n",
      "[[1.4343830156139423, 0.0, 0.0], [-0.04289927686635116, 0.32331069616248465, 0.0], [0.04787970075878745, -0.011751887289585384, 0.3124847192544564]]\n",
      "sigma = \n",
      "[[-0.34228661 10.49493889  1.85346767]\n",
      " [ 2.52647942 10.49493889  1.85346767]\n",
      " [-0.42808516 11.14156029  1.85346767]\n",
      " [-0.24652721 10.47143512  2.47843711]\n",
      " [-3.21105264 10.49493889  1.85346767]\n",
      " [-0.25648805  9.8483175   1.85346767]\n",
      " [-0.43804601 10.51844267  1.22849823]]\n",
      "sqP = \n",
      "[[1.4398931315802619, 0.0, 0.0], [-0.08600766732928161, 0.5502160010427491, 0.0], [0.03644400569373247, -0.006563878947179171, 0.5478213095216857]]\n",
      "m = \n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[ 0.01413032 10.30948766  1.78758865]\n",
      "P = \n",
      "[[ 2.05352994 -0.09794505  0.042458  ]\n",
      " [-0.09794505  0.13634002  0.02004755]\n",
      " [ 0.042458    0.02004755  0.10679559]]\n",
      "num = \n",
      "1104\n",
      "SENSOR 4 0.953865121238 -1.2816545328\n",
      "\n",
      "num = \n",
      "1105\n",
      "SENSOR 5 3.75050538274 -2.21025810438\n",
      "\n",
      "num = \n",
      "1106\n",
      "ODOMETRY 0.000643301288042 0.0998677942291 -0.202643646401\n",
      "\n",
      "sqP = \n",
      "[[1.4330142830965413, 0.0, 0.0], [-0.06834897168711689, 0.36286145648995577, 0.0], [0.029628456204146832, 0.060829351216799346, 0.3197147672101361]]\n",
      "sigma = \n",
      "[[ 0.01413032 10.30948766  1.78758865]\n",
      " [ 2.88015888 10.30948766  1.78758865]\n",
      " [-0.12256763 11.03521057  1.78758865]\n",
      " [ 0.07338723 10.43114636  2.42701818]\n",
      " [-2.85189825 10.30948766  1.78758865]\n",
      " [ 0.15082826  9.58376474  1.78758865]\n",
      " [-0.0451266  10.18782896  1.14815911]]\n",
      "sqP = \n",
      "[[1.5030234073048967, 0.0, 0.0], [-0.015502929357095026, 0.585201257620185, 0.0], [0.019706495246298156, 0.029974583668502242, 0.550524868927897]]\n",
      "m = \n",
      "[-0.37773447 10.29866256  1.65113178]\n",
      "P = \n",
      "[[ 2.25106217 -0.00449687  0.0307514 ]\n",
      " [-0.00449687  0.09936283 -0.0071165 ]\n",
      " [ 0.0307514  -0.0071165   0.10652046]]\n",
      "num = \n",
      "1107\n",
      "SENSOR 4 0.937678468196 -1.17091065111\n",
      "\n",
      "num = \n",
      "1108\n",
      "SENSOR 5 3.86489395394 -1.98135942803\n",
      "\n",
      "num = \n",
      "1109\n",
      "ODOMETRY -0.000515670443917 0.100048418713 -0.201085070412\n",
      "\n",
      "sqP = \n",
      "[[1.5003540161587607, 0.0, 0.0], [-0.0029972085376488737, 0.3152044572010418, 0.0], [0.020496094813072862, -0.022382519594207662, 0.3249606088606321]]\n",
      "sigma = \n",
      "[[-0.37773447 10.29866256  1.65113178]\n",
      " [ 2.62297356 10.29866256  1.65113178]\n",
      " [-0.38372888 10.92907147  1.65113178]\n",
      " [-0.33674228 10.25389752  2.30105299]\n",
      " [-3.3784425  10.29866256  1.65113178]\n",
      " [-0.37174005  9.66825364  1.65113178]\n",
      " [-0.41872666 10.3434276   1.00121056]]\n",
      "sqP = \n",
      "[[1.433855647369573, 0.0, 0.0], [-0.07699492447132769, 0.5479332866537208, 0.0], [0.034412451136221954, -0.012822330217623504, 0.5536581560891645]]\n",
      "m = \n",
      "[0.02102775 9.95213783 1.50686947]\n",
      "P = \n",
      "[[ 2.03578205 -0.08964037  0.0394428 ]\n",
      " [-0.08964037  0.14628232  0.02584348]\n",
      " [ 0.0394428   0.02584348  0.10511937]]\n",
      "num = \n",
      "1110\n",
      "SENSOR 4 1.03088856231 -1.02236401255\n",
      "\n",
      "num = \n",
      "1111\n",
      "SENSOR 5 3.61390947737 -1.83253751296\n",
      "\n",
      "num = \n",
      "1112\n",
      "ODOMETRY -0.00070658966091 0.0999211581655 -0.201215641668\n",
      "\n",
      "sqP = \n",
      "[[1.4268083437527623, 0.0, 0.0], [-0.06282579725887458, 0.37727342441063605, 0.0], [0.027644076963162435, 0.07310412344960754, 0.31466007523900097]]\n",
      "sigma = \n",
      "[[ 0.02102775  9.95213783  1.50686947]\n",
      " [ 2.87464444  9.95213783  1.50686947]\n",
      " [-0.10462384 10.70668468  1.50686947]\n",
      " [ 0.07631591 10.09834608  2.13618962]\n",
      " [-2.83258893  9.95213783  1.50686947]\n",
      " [ 0.14667935  9.19759098  1.50686947]\n",
      " [-0.0342604   9.80592959  0.87754932]]\n",
      "sqP = \n",
      "[[1.4968277541503763, 0.0, 0.0], [-0.014769255750950798, 0.595588345785433, 0.0], [0.019665426472002024, 0.03568744810708161, 0.5471212279339258]]\n",
      "m = \n",
      "[-0.30584     9.80004542  1.53945794]\n",
      "P = \n",
      "[[ 2.23461052 -0.0199661   0.02535125]\n",
      " [-0.0199661   0.19558486  0.03937583]\n",
      " [ 0.02535125  0.03937583  0.1028282 ]]\n",
      "num = \n",
      "1113\n",
      "SENSOR 8 4.85461136115 -0.441193806736\n",
      "\n",
      "num = \n",
      "1114\n",
      "SENSOR 4 1.05741780526 -0.941454125061\n",
      "\n",
      "num = \n",
      "1115\n",
      "SENSOR 5 3.87262435523 -1.67878747955\n",
      "\n",
      "num = \n",
      "1116\n",
      "ODOMETRY -0.000453651906009 0.100058053226 -0.0495339656427\n",
      "\n",
      "sqP = \n",
      "[[1.4948613706004032, 0.0, 0.0], [-0.013356486180112231, 0.4420480337870816, 0.0], [0.016958930298458394, 0.0895883301259119, 0.3074321530295206]]\n",
      "sigma = \n",
      "[[-0.30584     9.80004542  1.53945794]\n",
      " [ 2.68388274  9.80004542  1.53945794]\n",
      " [-0.33255297 10.68414148  1.53945794]\n",
      " [-0.27192214  9.97922208  2.15432224]\n",
      " [-3.29556274  9.80004542  1.53945794]\n",
      " [-0.27912703  8.91594935  1.53945794]\n",
      " [-0.33975786  9.62086876  0.92459363]]\n",
      "sqP = \n",
      "[[1.4341488569240162, 0.0, 0.0], [-0.08096074889633985, 0.6350946104679608, 0.0], [0.027907940759632318, 0.04319056929049139, 0.5418426241844498]]\n",
      "m = \n",
      "[0.08704785 9.60388564 1.37446497]\n",
      "P = \n",
      "[[ 2.03161478 -0.08175451  0.03067745]\n",
      " [-0.08175451  0.12834533  0.03852069]\n",
      " [ 0.03067745  0.03852069  0.10121052]]\n",
      "num = \n",
      "1117\n",
      "SENSOR 8 4.91108468826 -0.314178952957\n",
      "\n",
      "num = \n",
      "1118\n",
      "SENSOR 4 0.889803683381 -0.862587924879\n",
      "\n",
      "num = \n",
      "1119\n",
      "SENSOR 5 4.001907275 -1.75368141539\n",
      "\n",
      "num = \n",
      "1120\n",
      "ODOMETRY -0.000374433876798 0.0999632649737 0.000227424570168\n",
      "\n",
      "sqP = \n",
      "[[1.425347248971375, 0.0, 0.0], [-0.057357609541054115, 0.3536317824641948, 0.0], [0.021522793690547472, 0.11241971243082678, 0.29683176951514467]]\n",
      "sigma = \n",
      "[[ 0.08704785  9.60388564  1.37446497]\n",
      " [ 2.93774235  9.60388564  1.37446497]\n",
      " [-0.02766737 10.31114921  1.37446497]\n",
      " [ 0.13009344  9.82872507  1.96812851]\n",
      " [-2.76364665  9.60388564  1.37446497]\n",
      " [ 0.20176307  8.89662208  1.37446497]\n",
      " [ 0.04400226  9.37904622  0.78080143]]\n",
      "sqP = \n",
      "[[1.4951146799395614, 0.0, 0.0], [-0.013152928202481724, 0.5873290679712818, 0.0], [0.018150889500705765, 0.05512134510098383, 0.5350792114134889]]\n",
      "m = \n",
      "[-0.09026006  9.50746938  1.24944819]\n",
      "P = \n",
      "[[ 2.22826484 -0.01546407  0.02201635]\n",
      " [-0.01546407  0.12515507  0.03820247]\n",
      " [ 0.02201635  0.03820247  0.09585626]]\n",
      "num = \n",
      "1121\n",
      "SENSOR 8 4.89607963687 -0.383905377422\n",
      "\n",
      "num = \n",
      "1122\n",
      "SENSOR 4 1.03529311279 -1.08642084347\n",
      "\n",
      "num = \n",
      "1123\n",
      "SENSOR 5 3.6950450598 -1.62958875652\n",
      "\n",
      "num = \n",
      "1124\n",
      "ODOMETRY 0.000560387911335 0.100022258169 -0.000690144025918\n",
      "\n",
      "sqP = \n",
      "[[1.4927373660048446, 0.0, 0.0], [-0.010359538356992177, 0.35362091220920383, 0.0], [0.014748976844549925, 0.10846435860199417, 0.28961044358862653]]\n",
      "sigma = \n",
      "[[-0.09026006  9.50746938  1.24944819]\n",
      " [ 2.89521467  9.50746938  1.24944819]\n",
      " [-0.11097914 10.2147112   1.24944819]\n",
      " [-0.06076211  9.7243981   1.82866908]\n",
      " [-3.07573479  9.50746938  1.24944819]\n",
      " [-0.06954099  8.80022756  1.24944819]\n",
      " [-0.11975802  9.29054066  0.67022731]]\n",
      "sqP = \n",
      "[[1.558393306011038, 0.0, 0.0], [-0.0006485495712570938, 0.5866143513432673, 0.0], [0.010175091132088109, 0.05214796177482558, 0.5310606108604286]]\n",
      "m = \n",
      "[-0.11123224  9.5547837   1.3063754 ]\n",
      "P = \n",
      "[[ 2.42832631e+00 -8.28247982e-04  1.23747939e-02]\n",
      " [-8.28247982e-04  1.31801499e-01  3.72237195e-02]\n",
      " [ 1.23747939e-02  3.72237195e-02  9.09507044e-02]]\n",
      "num = \n",
      "1125\n",
      "SENSOR 8 4.77811944922 -0.344660362116\n",
      "\n",
      "num = \n",
      "1126\n",
      "SENSOR 4 0.6835522748 -1.12873248686\n",
      "\n",
      "num = \n",
      "1127\n",
      "SENSOR 5 3.73078201728 -1.73731680832\n",
      "\n",
      "num = \n",
      "1128\n",
      "ODOMETRY 0.000302602191039 0.0999189321218 -0.000444970990895\n",
      "\n",
      "sqP = \n",
      "[[1.5583087994773408, 0.0, 0.0], [-0.0005315043990298022, 0.363044372163232, 0.0], [0.007941169224392343, 0.10254377450160126, 0.2835002937894072]]\n",
      "sigma = \n",
      "[[-0.11123224  9.5547837   1.3063754 ]\n",
      " [ 3.00538536  9.5547837   1.3063754 ]\n",
      " [-0.11229525 10.28087245  1.3063754 ]\n",
      " [-0.09534991  9.75987125  1.87337599]\n",
      " [-3.22784984  9.5547837   1.3063754 ]\n",
      " [-0.11016924  8.82869496  1.3063754 ]\n",
      " [-0.12711458  9.34969615  0.73937481]]\n",
      "sqP = \n",
      "[[1.4320510145271845, 0.0, 0.0], [-0.08133367429424955, 0.5857680567474023, 0.0], [0.010652088133998778, 0.04985093814651307, 0.5275557666786842]]\n",
      "m = \n",
      "[0.56402095 9.40947796 1.33046756]\n",
      "P = \n",
      "[[ 2.02847756 -0.08928993  0.00234749]\n",
      " [-0.08928993  0.13125086  0.02540952]\n",
      " [ 0.00234749  0.02540952  0.10321498]]\n",
      "num = \n",
      "1129\n",
      "SENSOR 8 4.69613208187 -0.387130423206\n",
      "\n",
      "num = \n",
      "1130\n",
      "SENSOR 4 0.787992521116 -1.29156833847\n",
      "\n",
      "num = \n",
      "1131\n",
      "SENSOR 5 3.83067456717 -1.73774870776\n",
      "\n",
      "num = \n",
      "1132\n",
      "ODOMETRY 0.00033245853007 0.099983722446 3.29125408185e-05\n",
      "\n",
      "sqP = \n",
      "[[1.4242463142014579, 0.0, 0.0], [-0.06269275970168321, 0.3568199574517913, 0.0], [0.0016482307380622956, 0.07150064062412784, 0.31320906828885303]]\n",
      "sigma = \n",
      "[[ 0.56402095  9.40947796  1.33046756]\n",
      " [ 3.41251358  9.40947796  1.33046756]\n",
      " [ 0.43863543 10.12311787  1.33046756]\n",
      " [ 0.56731741  9.55247924  1.95688569]\n",
      " [-2.28447168  9.40947796  1.33046756]\n",
      " [ 0.68940647  8.69583804  1.33046756]\n",
      " [ 0.56072449  9.26647667  0.70404942]]\n",
      "sqP = \n",
      "[[1.4408194995137213, 0.0, 0.0], [0.052665262195585864, 0.5806968039095282, 0.0], [0.04205516772096393, 0.03685083864485216, 0.5452368627668218]]\n",
      "m = \n",
      "[-0.43962098  9.33352978  1.39169832]\n",
      "P = \n",
      "[[2.05640278 0.05582662 0.05280758]\n",
      " [0.05582662 0.12889844 0.02694111]\n",
      " [0.05280758 0.02694111 0.08925255]]\n",
      "num = \n",
      "1133\n",
      "SENSOR 8 4.43987229987 -0.336598652401\n",
      "\n",
      "num = \n",
      "1134\n",
      "SENSOR 4 0.824851032983 -1.37173579521\n",
      "\n",
      "num = \n",
      "1135\n",
      "SENSOR 5 3.9011475333 -1.83111676621\n",
      "\n",
      "num = \n",
      "1136\n",
      "ODOMETRY 0.0003202499917 0.0998020171825 -0.000105496843961\n",
      "\n",
      "sqP = \n",
      "[[1.4340163107084427, 0.0, 0.0], [0.038930254905904016, 0.35690737215035667, 0.0], [0.036824951574431344, 0.07146812767399577, 0.28773038547541624]]\n",
      "sigma = \n",
      "[[-0.43962098  9.33352978  1.39169832]\n",
      " [ 2.42841165  9.33352978  1.39169832]\n",
      " [-0.36176047 10.04734452  1.39169832]\n",
      " [-0.36597107  9.47646604  1.96715909]\n",
      " [-3.3076536   9.33352978  1.39169832]\n",
      " [-0.51748149  8.61971504  1.39169832]\n",
      " [-0.51327088  9.19059353  0.81623755]]\n",
      "sqP = \n",
      "[[1.439489923483441, 0.0, 0.0], [-0.061591367968897234, 0.5796921592798095, 0.0], [0.04050032377784692, 0.038484565698957904, 0.5320336779423468]]\n",
      "m = \n",
      "[0.18883692 9.14164951 1.3229424 ]\n",
      "P = \n",
      "[[ 2.05046132 -0.07031778  0.04348487]\n",
      " [-0.07031778  0.1323724   0.02794687]\n",
      " [ 0.04348487  0.02794687  0.09299013]]\n",
      "num = \n",
      "1137\n",
      "SENSOR 8 4.2295439355 -0.31030833621\n",
      "\n",
      "num = \n",
      "1138\n",
      "SENSOR 4 0.562121580647 -1.48855015673\n",
      "\n",
      "num = \n",
      "1139\n",
      "SENSOR 5 4.05047851622 -1.85523162918\n",
      "\n",
      "num = \n",
      "1140\n",
      "ODOMETRY -0.00100295830048 0.100070287738 -0.000357513844835\n",
      "\n",
      "sqP = \n",
      "[[1.4319431986717186, 0.0, 0.0], [-0.049106545548077994, 0.3605009607781784, 0.0], [0.03036773055106654, 0.08165893761452692, 0.29223234904252043]]\n",
      "sigma = \n",
      "[[ 0.18883692  9.14164951  1.3229424 ]\n",
      " [ 3.05272332  9.14164951  1.3229424 ]\n",
      " [ 0.09062383  9.86265143  1.3229424 ]\n",
      " [ 0.24957238  9.30496738  1.9074071 ]\n",
      " [-2.67504948  9.14164951  1.3229424 ]\n",
      " [ 0.28705001  8.42064759  1.3229424 ]\n",
      " [ 0.12810146  8.97833163  0.7384777 ]]\n",
      "sqP = \n",
      "[[1.5012644590799717, 0.0, 0.0], [-0.01263540157881998, 0.5864850422035855, 0.0], [0.018944622324185197, 0.04046195481900355, 0.5344234171626001]]\n",
      "m = \n",
      "[0.09816874 8.90336686 1.29686973]\n",
      "P = \n",
      "[[ 2.24773335 -0.0169352   0.02159893]\n",
      " [-0.0169352   0.1312737   0.02571019]\n",
      " [ 0.02159893  0.02571019  0.09139642]]\n",
      "num = \n",
      "1141\n",
      "SENSOR 8 4.14141953883 -0.401434359312\n",
      "\n",
      "num = \n",
      "1142\n",
      "SENSOR 4 0.698270524653 -1.66266898697\n",
      "\n",
      "num = \n",
      "1143\n",
      "SENSOR 5 4.02646711957 -1.85655804032\n",
      "\n",
      "num = \n",
      "1144\n",
      "ODOMETRY -0.000188063237756 0.0999460648438 -0.000367434508236\n",
      "\n",
      "sqP = \n",
      "[[1.4992442612279606, 0.0, 0.0], [-0.011295822296838376, 0.36214100387161785, 0.0], [0.014406543107705866, 0.07144433400375887, 0.29340174290815185]]\n",
      "sigma = \n",
      "[[ 0.09816874  8.90336686  1.29686973]\n",
      " [ 3.09665726  8.90336686  1.29686973]\n",
      " [ 0.07557709  9.62764887  1.29686973]\n",
      " [ 0.12698182  9.04625553  1.88367321]\n",
      " [-2.90031978  8.90336686  1.29686973]\n",
      " [ 0.12076038  8.17908485  1.29686973]\n",
      " [ 0.06935565  8.76047819  0.71006624]]\n",
      "sqP = \n",
      "[[1.5646304672064422, 0.0, 0.0], [-0.002622053895125696, 0.5862301807061702, 0.0], [0.009528229402826388, 0.036424844074475556, 0.5344460761806344]]\n",
      "m = \n",
      "[0.08769447 8.77416447 1.28078291]\n",
      "P = \n",
      "[[ 2.44795319 -0.00350948  0.01133309]\n",
      " [-0.00350948  0.12847591  0.01774744]\n",
      " [ 0.01133309  0.01774744  0.09141743]]\n",
      "num = \n",
      "1145\n",
      "SENSOR 8 4.49059154755 -0.359642792629\n",
      "\n",
      "num = \n",
      "1146\n",
      "SENSOR 4 0.829352455695 -1.82738715515\n",
      "\n",
      "num = \n",
      "1147\n",
      "SENSOR 5 3.9198043194 -1.85847185083\n",
      "\n",
      "num = \n",
      "1148\n",
      "ODOMETRY 0.000145108773369 0.0999890308492 -0.0008287350519\n",
      "\n",
      "sqP = \n",
      "[[1.564593617218284, 0.0, 0.0], [-0.0022430609173170464, 0.35842834470583734, 0.0], [0.007243473535987269, 0.04955994016513477, 0.29817575000459606]]\n",
      "sigma = \n",
      "[[ 0.08769447  8.77416447  1.28078291]\n",
      " [ 3.21688171  8.77416447  1.28078291]\n",
      " [ 0.08320835  9.49102116  1.28078291]\n",
      " [ 0.10218142  8.87328435  1.87713441]\n",
      " [-3.04149276  8.77416447  1.28078291]\n",
      " [ 0.0921806   8.05730778  1.28078291]\n",
      " [ 0.07320753  8.67504459  0.68443141]]\n",
      "sqP = \n",
      "[[1.4320079601878934, 0.0, 0.0], [0.08159433733921471, 0.5759436119570912, 0.0], [0.008695154567801342, 0.02544870006701523, 0.537283694456864]]\n",
      "m = \n",
      "[-1.07755306  8.90038058  1.29549154]\n",
      "P = \n",
      "[[1.94627459 0.07485412 0.01864216]\n",
      " [0.07485412 0.13240512 0.01103017]\n",
      " [0.01864216 0.01103017 0.09481074]]\n",
      "num = \n",
      "1149\n",
      "SENSOR 8 4.0305518916 -0.410490083997\n",
      "\n",
      "num = \n",
      "1150\n",
      "SENSOR 4 0.73104561841 -1.91623346334\n",
      "\n",
      "num = \n",
      "1151\n",
      "SENSOR 5 3.95455389546 -1.81930736734\n",
      "\n",
      "num = \n",
      "1152\n",
      "ODOMETRY 0.000317286436698 0.0999382739112 -0.000139268224043\n",
      "\n",
      "sqP = \n",
      "[[1.39508945601832, 0.0, 0.0], [0.053655426547402595, 0.35989750894079525, 0.0], [0.01336269996667816, 0.028655911080582852, 0.3062858466146196]]\n",
      "sigma = \n",
      "[[-1.07755306  8.90038058  1.29549154]\n",
      " [ 1.71262586  8.90038058  1.29549154]\n",
      " [-0.9702422   9.6201756   1.29549154]\n",
      " [-1.05082766  8.95769241  1.90806323]\n",
      " [-3.86773197  8.90038058  1.29549154]\n",
      " [-1.18486391  8.18058557  1.29549154]\n",
      " [-1.10427846  8.84306876  0.68291985]]\n",
      "sqP = \n",
      "[[1.443773249575824, 0.0, 0.0], [-0.025439973145967826, 0.5787857765899677, 0.0], [0.07153267670734408, 0.01570663109244674, 0.5425524817693955]]\n",
      "m = \n",
      "[-0.50473706  8.81397474  1.38980903]\n",
      "P = \n",
      "[[ 2.06220088 -0.02253518  0.08046126]\n",
      " [-0.02253518  0.09194605  0.00280762]\n",
      " [ 0.08046126  0.00280762  0.09658311]]\n",
      "num = \n",
      "1153\n",
      "SENSOR 8 3.94074309758 -0.449033720052\n",
      "\n",
      "num = \n",
      "1154\n",
      "SENSOR 4 0.964327453377 -2.01100457015\n",
      "\n",
      "num = \n",
      "1155\n",
      "SENSOR 5 4.12377150794 -1.90702070002\n",
      "\n",
      "num = \n",
      "1156\n",
      "ODOMETRY -0.000489035127382 0.10010816554 -0.000185002568801\n",
      "\n",
      "sqP = \n",
      "[[1.43603651600289, 0.0, 0.0], [-0.015692624408990805, 0.30281973008395546, 0.0], [0.05603009145417512, 0.012175149860026132, 0.3054431237330697]]\n",
      "sigma = \n",
      "[[-0.50473706  8.81397474  1.38980903]\n",
      " [ 2.36733597  8.81397474  1.38980903]\n",
      " [-0.53612231  9.4196142   1.38980903]\n",
      " [-0.39267688  8.83832504  2.00069528]\n",
      " [-3.37681009  8.81397474  1.38980903]\n",
      " [-0.47335181  8.20833528  1.38980903]\n",
      " [-0.61679724  8.78962444  0.77892278]]\n",
      "sqP = \n",
      "[[1.4394779127927448, 0.0, 0.0], [-0.07354744867491009, 0.5400165023976986, 0.0], [0.04977562818452602, 0.009052633697881687, 0.5436626261106444]]\n",
      "m = \n",
      "[0.12154296 8.5721387  1.25782389]\n",
      "P = \n",
      "[[ 2.05097178 -0.0888074   0.05142219]\n",
      " [-0.0888074   0.12098447  0.00627554]\n",
      " [ 0.05142219  0.00627554  0.09715661]]\n",
      "num = \n",
      "1157\n",
      "SENSOR 8 3.91534330666 -0.437399649948\n",
      "\n",
      "num = \n",
      "1158\n",
      "SENSOR 4 0.962844439912 -2.09903655675\n",
      "\n",
      "num = \n",
      "1159\n",
      "SENSOR 5 4.0517244807 -1.90944618344\n",
      "\n",
      "num = \n",
      "1160\n",
      "ODOMETRY -0.000448212051337 0.100133168601 -0.0012892610486\n",
      "\n",
      "sqP = \n",
      "[[1.4321214278487684, 0.0, 0.0], [-0.06201108543446083, 0.34225589391515504, 0.0], [0.03590630523900805, 0.02484143202677073, 0.3086263884136854]]\n",
      "sigma = \n",
      "[[ 1.21542965e-01  8.57213870e+00  1.25782389e+00]\n",
      " [ 2.98578582e+00  8.57213870e+00  1.25782389e+00]\n",
      " [-2.47920589e-03  9.25665049e+00  1.25782389e+00]\n",
      " [ 1.93355575e-01  8.62182156e+00  1.87507667e+00]\n",
      " [-2.74269989e+00  8.57213870e+00  1.25782389e+00]\n",
      " [ 2.45565136e-01  7.88762691e+00  1.25782389e+00]\n",
      " [ 4.97303545e-02  8.52245584e+00  6.40571112e-01]]\n",
      "sqP = \n",
      "[[1.5020340946783706, 0.0, 0.0], [-0.015155330053584775, 0.5701945403540267, 0.0], [0.02068838949420272, 0.011691620888590916, 0.5451851193991806]]\n",
      "m = \n",
      "[0.01948126 8.48363708 1.26732459]\n",
      "P = \n",
      "[[ 2.25058863e+00 -2.10246967e-02  2.21778601e-02]\n",
      " [-2.10246967e-02  1.18854519e-01  2.18661612e-03]\n",
      " [ 2.21778601e-02  2.18661612e-03  9.62985316e-02]]\n",
      "num = \n",
      "1161\n",
      "SENSOR 8 3.8900635959 -0.450094707651\n",
      "\n",
      "num = \n",
      "1162\n",
      "SENSOR 4 1.15499811144 -2.14063500493\n",
      "\n",
      "num = \n",
      "1163\n",
      "SENSOR 5 4.15361685125 -1.97034676565\n",
      "\n",
      "num = \n",
      "1164\n",
      "ODOMETRY -0.000333404857971 0.0999912335863 0.000192681272112\n",
      "\n",
      "sqP = \n",
      "[[1.5001961980611722, 0.0, 0.0], [-0.014014631382771946, 0.34446786321012285, 0.0], [0.01478330641903818, 0.006949265718726831, 0.3098898081664221]]\n",
      "sigma = \n",
      "[[ 1.94812612e-02  8.48363708e+00  1.26732459e+00]\n",
      " [ 3.01987366e+00  8.48363708e+00  1.26732459e+00]\n",
      " [-8.54800156e-03  9.17257281e+00  1.26732459e+00]\n",
      " [ 4.90478740e-02  8.49753561e+00  1.88710421e+00]\n",
      " [-2.98091113e+00  8.48363708e+00  1.26732459e+00]\n",
      " [ 4.75105240e-02  7.79470136e+00  1.26732459e+00]\n",
      " [-1.00853516e-02  8.46973855e+00  6.47544976e-01]]\n",
      "sqP = \n",
      "[[1.5655681360830866, 0.0, 0.0], [-0.003146985759208346, 0.5710998968214309, 0.0], [0.009703264556785354, 0.0032270291208582524, 0.544886012910434]]\n",
      "m = \n",
      "[8.09136591e-03 8.38341931e+00 1.19077876e+00]\n",
      "P = \n",
      "[[ 2.45090126e+00 -4.09592291e-03  1.15422250e-02]\n",
      " [-4.09592291e-03  1.16332787e-01 -1.02104729e-03]\n",
      " [ 1.15422250e-02 -1.02104729e-03  9.78583677e-02]]\n",
      "num = \n",
      "1165\n",
      "SENSOR 8 3.77038644114 -0.564054467112\n",
      "\n",
      "num = \n",
      "1166\n",
      "SENSOR 4 0.97476218836 -2.24080497152\n",
      "\n",
      "num = \n",
      "1167\n",
      "SENSOR 5 4.17413980453 -1.93610976395\n",
      "\n",
      "num = \n",
      "1168\n",
      "ODOMETRY 0.000116797154257 0.100137172993 -0.000390404927584\n",
      "\n",
      "sqP = \n",
      "[[1.565535453428152, 0.0, 0.0], [-0.0026163079872487866, 0.3410658904079898, 0.0], [0.007372701104839638, -0.002937139320552135, 0.31272253551093343]]\n",
      "sigma = \n",
      "[[ 8.09136591e-03  8.38341931e+00  1.19077876e+00]\n",
      " [ 3.13916227e+00  8.38341931e+00  1.19077876e+00]\n",
      " [ 2.85874994e-03  9.06555109e+00  1.19077876e+00]\n",
      " [ 2.28367681e-02  8.37754504e+00  1.81622383e+00]\n",
      " [-3.12297954e+00  8.38341931e+00  1.19077876e+00]\n",
      " [ 1.33239819e-02  7.70128753e+00  1.19077876e+00]\n",
      " [-6.65403630e-03  8.38929359e+00  5.65333689e-01]]\n",
      "sqP = \n",
      "[[1.6281776493151952, 0.0, 0.0], [-0.0005653095148961629, 0.5690634232652066, 0.0], [0.0019256039054289084, -0.001673692895546564, 0.5461238556930537]]\n",
      "m = \n",
      "[0.1064192  8.32267336 1.22879543]\n",
      "P = \n",
      "[[ 2.64683828e+00  2.19667109e-03  3.80104421e-03]\n",
      " [ 2.19667109e-03  1.15405262e-01 -4.65329724e-03]\n",
      " [ 3.80104421e-03 -4.65329724e-03  9.66695873e-02]]\n",
      "num = \n",
      "1169\n",
      "SENSOR 8 3.63226086454 -0.540987639383\n",
      "\n",
      "num = \n",
      "1170\n",
      "SENSOR 4 1.10417906905 -2.38354517123\n",
      "\n",
      "num = \n",
      "1171\n",
      "SENSOR 5 4.30329593999 -2.0340943866\n",
      "\n",
      "num = \n",
      "1172\n",
      "SENSOR 7 4.94033277705 -0.757833609985\n",
      "\n",
      "num = \n",
      "1173\n",
      "ODOMETRY -0.000590074857926 0.0999133906589 -0.000728548241666\n",
      "\n",
      "sqP = \n",
      "[[1.6269106565513494, 0.0, 0.0], [0.0013502100294819875, 0.33971081653892005, 0.0], [0.0023363570646380595, -0.01370710495511673, 0.3106062523502931]]\n",
      "sigma = \n",
      "[[ 0.1064192   8.32267336  1.22879543]\n",
      " [ 3.36024052  8.32267336  1.22879543]\n",
      " [ 0.10911962  9.002095    1.22879543]\n",
      " [ 0.11109192  8.29525915  1.85000794]\n",
      " [-3.14740211  8.32267336  1.22879543]\n",
      " [ 0.10371878  7.64325173  1.22879543]\n",
      " [ 0.10174649  8.35008757  0.60758293]]\n",
      "sqP = \n",
      "[[1.5793254363125497, 0.0, 0.0], [-0.0003017472235927275, 0.5683056550060757, 0.0], [0.005807288450264468, -0.006042824088413092, 0.5446707039403067]]\n",
      "m = \n",
      "[0.12018184 8.1748923  1.20334153]\n",
      "P = \n",
      "[[2.49416090e+00 1.62234237e-04 7.15882437e-03]\n",
      " [1.62234237e-04 9.18376349e-02 1.31876773e-02]\n",
      " [7.15882437e-03 1.31876773e-02 8.02898888e-02]]\n",
      "num = \n",
      "1174\n",
      "SENSOR 8 3.48279289223 -0.442671033673\n",
      "\n",
      "num = \n",
      "1175\n",
      "SENSOR 4 1.21367576112 -2.38923397243\n",
      "\n",
      "num = \n",
      "1176\n",
      "SENSOR 5 4.11351339144 -2.12124027897\n",
      "\n",
      "num = \n",
      "1177\n",
      "SENSOR 7 4.9471083243 -0.863368530901\n",
      "\n",
      "num = \n",
      "1178\n",
      "ODOMETRY 0.000666027634388 0.0999961749585 -0.00041098921494\n",
      "\n",
      "sqP = \n",
      "[[1.5792912657753513, 0.0, 0.0], [0.00010272597638980327, 0.30304723114864995, 0.0], [0.004532934819066258, 0.04351536762183519, 0.2799567004001496]]\n",
      "sigma = \n",
      "[[ 0.12018184  8.1748923   1.20334153]\n",
      " [ 3.27876437  8.1748923   1.20334153]\n",
      " [ 0.12038729  8.78098676  1.20334153]\n",
      " [ 0.12924771  8.26192303  1.76325493]\n",
      " [-3.03840069  8.1748923   1.20334153]\n",
      " [ 0.11997639  7.56879784  1.20334153]\n",
      " [ 0.11111597  8.08786156  0.64342813]]\n",
      "sqP = \n",
      "[[1.432006825138496, 0.0, 0.0], [0.08176407832052672, 0.5426045449613629, 0.0], [0.010688591564632458, 0.02248898722331142, 0.5273660880624813]]\n",
      "m = \n",
      "[-1.02598162  8.11341228  1.30992074]\n",
      "P = \n",
      "[[1.99521125 0.0845949  0.03126025]\n",
      " [0.0845949  0.09306106 0.01038707]\n",
      " [0.03126025 0.01038707 0.06370192]]\n",
      "num = \n",
      "1179\n",
      "SENSOR 8 3.31329949365 -0.442208158787\n",
      "\n",
      "num = \n",
      "1180\n",
      "SENSOR 4 1.14087202904 -2.42970915883\n",
      "\n",
      "num = \n",
      "1181\n",
      "SENSOR 5 4.28986062879 -2.13951007973\n",
      "\n",
      "num = \n",
      "1182\n",
      "SENSOR 7 4.50725975692 -0.916450378366\n",
      "\n",
      "num = \n",
      "1183\n",
      "ODOMETRY 0.000123003750952 0.0998768096926 -0.000338091992771\n",
      "\n",
      "sqP = \n",
      "[[1.4125194689710014, 0.0, 0.0], [0.05988937187291824, 0.29912259475695535, 0.0], [0.022130846959094647, 0.030294163983840074, 0.2495884885590268]]\n",
      "sigma = \n",
      "[[-1.02598162  8.11341228  1.30992074]\n",
      " [ 1.79905732  8.11341228  1.30992074]\n",
      " [-0.90620288  8.71165747  1.30992074]\n",
      " [-0.98171993  8.1740006   1.80909771]\n",
      " [-3.85102056  8.11341228  1.30992074]\n",
      " [-1.14576037  7.51516709  1.30992074]\n",
      " [-1.07024332  8.05282395  0.81074376]]\n",
      "sqP = \n",
      "[[1.442109184554803, 0.0, 0.0], [-0.029604476658057582, 0.542961078041665, 0.0], [0.07109517654508725, 0.014864715919225073, 0.5127039513080874]]\n",
      "m = \n",
      "[-0.42693804  7.90078008  1.37516607]\n",
      "P = \n",
      "[[ 2.0586734  -0.0351108   0.07398175]\n",
      " [-0.0351108   0.08371215  0.00770566]\n",
      " [ 0.07398175  0.00770566  0.08271481]]\n",
      "num = \n",
      "1184\n",
      "SENSOR 8 3.58031102591 -0.631152189894\n",
      "\n",
      "num = \n",
      "1185\n",
      "SENSOR 4 1.28631748205 -2.52280355497\n",
      "\n",
      "num = \n",
      "1186\n",
      "SENSOR 5 4.29694827231 -2.13689954352\n",
      "\n",
      "num = \n",
      "1187\n",
      "SENSOR 7 4.75408087282 -0.865684520747\n",
      "\n",
      "num = \n",
      "1188\n",
      "ODOMETRY -0.000346457382135 0.100082898443 0.000286890929699\n",
      "\n",
      "sqP = \n",
      "[[1.434807793388391, 0.0, 0.0], [-0.024470730848221663, 0.2882938377487044, 0.0], [0.05156212990299583, 0.031105133269241473, 0.28122700791734573]]\n",
      "sigma = \n",
      "[[-0.42693804  7.90078008  1.37516607]\n",
      " [ 2.44267754  7.90078008  1.37516607]\n",
      " [-0.4758795   8.47736776  1.37516607]\n",
      " [-0.32381378  7.96299035  1.93762008]\n",
      " [-3.29655363  7.90078008  1.37516607]\n",
      " [-0.37799658  7.32419241  1.37516607]\n",
      " [-0.5300623   7.83856982  0.81271205]]\n",
      "sqP = \n",
      "[[1.4395489489179194, 0.0, 0.0], [-0.07717392447592089, 0.532607859844509, 0.0], [0.04230328889413835, 0.01780786231268125, 0.5301157147480065]]\n",
      "m = \n",
      "[2.10759985 7.91778903 1.31206101]\n",
      "P = \n",
      "[[ 0.70782371 -0.04800511  0.00603698]\n",
      " [-0.04800511  0.08159914  0.00411236]\n",
      " [ 0.00603698  0.00411236  0.08490879]]\n",
      "num = \n",
      "1189\n",
      "SENSOR 8 3.24455116705 -0.576310948619\n",
      "\n",
      "num = \n",
      "1190\n",
      "SENSOR 4 1.27283382282 -2.55880342549\n",
      "\n",
      "num = \n",
      "1191\n",
      "SENSOR 5 4.41483458904 -2.05309918925\n",
      "\n",
      "num = \n",
      "1192\n",
      "SENSOR 7 4.63121738461 -0.874521274014\n",
      "\n",
      "num = \n",
      "1193\n",
      "ODOMETRY -0.000955326333333 0.100096022413 -0.000648521461897\n",
      "\n",
      "sqP = \n",
      "[[0.8413225973659529, 0.0, 0.0], [-0.057059094216435076, 0.2798989116103266, 0.0], [0.007175585611443339, 0.01615507905569093, 0.290854451310034]]\n",
      "sigma = \n",
      "[[2.10759985 7.91778903 1.31206101]\n",
      " [3.79024504 7.91778903 1.31206101]\n",
      " [1.99348166 8.47758685 1.31206101]\n",
      " [2.12195102 7.95009919 1.89376991]\n",
      " [0.42495465 7.91778903 1.31206101]\n",
      " [2.22171804 7.35799121 1.31206101]\n",
      " [2.09324868 7.88547887 0.7303521 ]]\n",
      "sqP = \n",
      "[[1.6081013576242307, 0.0, 0.0], [-0.011433590555676405, 0.5325886518445431, 0.0], [0.047680434962437145, 0.011188728208952739, 0.5332415876878468]]\n",
      "m = \n",
      "[1.74703184 7.69888492 1.46835405]\n",
      "P = \n",
      "[[ 2.56977855 -0.00885934  0.05554952]\n",
      " [-0.00885934  0.07415353  0.00730998]\n",
      " [ 0.05554952  0.00730998  0.08497912]]\n",
      "num = \n",
      "1194\n",
      "SENSOR 8 3.24825762533 -0.599421325277\n",
      "\n",
      "num = \n",
      "1195\n",
      "SENSOR 4 1.50961115908 -2.52257610457\n",
      "\n",
      "num = \n",
      "1196\n",
      "SENSOR 5 4.51886453403 -2.10172063614\n",
      "\n",
      "num = \n",
      "1197\n",
      "SENSOR 7 4.67172096933 -0.894687907489\n",
      "\n",
      "num = \n",
      "1198\n",
      "ODOMETRY 0.000425896177498 0.100141662167 0.000472712559654\n",
      "\n",
      "sqP = \n",
      "[[1.6030528829197217, 0.0, 0.0], [-0.005526544439263018, 0.27225536560594066, 0.0], [0.03465233105504135, 0.027553135365455506, 0.28813045877105886]]\n",
      "sigma = \n",
      "[[ 1.74703184  7.69888492  1.46835405]\n",
      " [ 4.9531376   7.69888492  1.46835405]\n",
      " [ 1.73597875  8.24339565  1.46835405]\n",
      " [ 1.8163365   7.75399119  2.04461497]\n",
      " [-1.45907393  7.69888492  1.46835405]\n",
      " [ 1.75808492  7.15437419  1.46835405]\n",
      " [ 1.67772717  7.64377865  0.89209313]]\n",
      "sqP = \n",
      "[[1.4327674749792012, 0.0, 0.0], [-0.01472860980445807, 0.5244269707432541, 0.0], [0.08789189663119229, 0.013304164325496932, 0.5310690868886623]]\n",
      "m = \n",
      "[2.00217316 7.57627182 1.49761252]\n",
      "P = \n",
      "[[ 0.65368399 -0.00994044  0.08149318]\n",
      " [-0.00994044  0.06850327  0.00672163]\n",
      " [ 0.08149318  0.00672163  0.09333516]]\n",
      "num = \n",
      "1199\n",
      "SENSOR 8 3.23972718873 -0.58466657353\n",
      "\n",
      "num = \n",
      "1200\n",
      "SENSOR 4 1.74083901202 -2.64636281352\n",
      "\n",
      "num = \n",
      "1201\n",
      "SENSOR 5 4.36236644466 -2.15541169775\n",
      "\n",
      "num = \n",
      "1202\n",
      "SENSOR 7 4.54743703627 -0.983253835746\n",
      "\n",
      "num = \n",
      "1203\n",
      "ODOMETRY 0.000431170979157 0.100025186646 -9.68768691897e-05\n",
      "\n",
      "sqP = \n",
      "[[0.8085072632697223, 0.0, 0.0], [-0.012294801450572167, 0.26144235988927284, 0.0], [0.10079461981169525, 0.030449860978112517, 0.28678982969413636]]\n",
      "sigma = \n",
      "[[2.00217316 7.57627182 1.49761252]\n",
      " [3.61918769 7.57627182 1.49761252]\n",
      " [1.97758356 8.09915654 1.49761252]\n",
      " [2.2037624  7.63717155 2.07119218]\n",
      " [0.38515863 7.57627182 1.49761252]\n",
      " [2.02676276 7.0533871  1.49761252]\n",
      " [1.80058392 7.5153721  0.92403286]]\n",
      "sqP = \n",
      "[[1.6254064596586404, 0.0, 0.0], [0.004855868028344329, 0.5213342956593486, 0.0], [0.061594754445006256, 0.011598345673096238, 0.5273001197433115]]\n",
      "m = \n",
      "[1.87809182 7.5061899  1.51377705]\n",
      "P = \n",
      "[[2.60775952 0.01047525 0.07272668]\n",
      " [0.01047525 0.06690845 0.00751616]\n",
      " [0.07272668 0.00751616 0.09103402]]\n",
      "num = \n",
      "1204\n",
      "SENSOR 8 2.94713147949 -0.538730214309\n",
      "\n",
      "num = \n",
      "1205\n",
      "SENSOR 4 1.59532499948 -2.64189895638\n",
      "\n",
      "num = \n",
      "1206\n",
      "SENSOR 5 4.3940471103 -2.19522353029\n",
      "\n",
      "num = \n",
      "1207\n",
      "SENSOR 7 4.58101568205 -0.938466709816\n",
      "\n",
      "num = \n",
      "1208\n",
      "ODOMETRY -0.00126089300156 0.100003408847 -0.000253955323998\n",
      "\n",
      "sqP = \n",
      "[[1.6148558825279862, 0.0, 0.0], [0.006486801266791002, 0.25858532090709435, 0.0], [0.04503601662118785, 0.02793668839303437, 0.297027468311877]]\n",
      "sigma = \n",
      "[[ 1.87809182  7.5061899   1.51377705]\n",
      " [ 5.10780359  7.5061899   1.51377705]\n",
      " [ 1.89106542  8.02336054  1.51377705]\n",
      " [ 1.96816385  7.56206328  2.10783199]\n",
      " [-1.35161994  7.5061899   1.51377705]\n",
      " [ 1.86511822  6.98901926  1.51377705]\n",
      " [ 1.78801979  7.45031652  0.91972211]]\n",
      "sqP = \n",
      "[[1.4333744769038657, 0.0, 0.0], [-0.022796267882104936, 0.5169999915922311, 0.0], [0.08747299394889876, 0.013126761326801396, 0.5351404186274443]]\n",
      "m = \n",
      "[2.11045362 7.44369838 1.60227136]\n",
      "P = \n",
      "[[ 0.65368432 -0.01796785  0.07991987]\n",
      " [-0.01796785  0.06682041  0.00558889]\n",
      " [ 0.07991987  0.00558889  0.09486537]]\n",
      "num = \n",
      "1209\n",
      "SENSOR 8 3.01049631895 -0.647455670209\n",
      "\n",
      "num = \n",
      "1210\n",
      "SENSOR 4 1.74223188952 -2.64154107534\n",
      "\n",
      "num = \n",
      "1211\n",
      "SENSOR 5 4.6784242394 -2.17124070517\n",
      "\n",
      "num = \n",
      "1212\n",
      "SENSOR 7 4.2652733295 -0.996316702854\n",
      "\n",
      "num = \n",
      "1213\n",
      "ODOMETRY -0.000120654780893 0.0999758007847 -7.69033417334e-05\n",
      "\n",
      "sqP = \n",
      "[[0.8085074649354166, 0.0, 0.0], [-0.022223476846611974, 0.25753936757638424, 0.0], [0.09884865032344284, 0.03023090137780919, 0.2901385915085887]]\n",
      "sigma = \n",
      "[[2.11045362 7.44369838 1.60227136]\n",
      " [3.72746855 7.44369838 1.60227136]\n",
      " [2.06600667 7.95877712 1.60227136]\n",
      " [2.30815092 7.50416019 2.18254855]\n",
      " [0.49343869 7.44369838 1.60227136]\n",
      " [2.15490057 6.92861965 1.60227136]\n",
      " [1.91275632 7.38323658 1.02199418]]\n",
      "sqP = \n",
      "[[1.625392282716143, 0.0, 0.0], [-0.0014254379681451798, 0.5197667280248908, 0.0], [0.0616743960927784, 0.01309196335683857, 0.528639087089619]]\n",
      "m = \n",
      "[1.93452999 7.29738292 1.54455119]\n",
      "P = \n",
      "[[2.60759246e+00 2.36332509e-03 7.31564851e-02]\n",
      " [2.36332509e-03 6.48497383e-02 7.73156442e-03]\n",
      " [7.31564851e-02 7.73156442e-03 9.39504286e-02]]\n",
      "num = \n",
      "1214\n",
      "SENSOR 8 2.81105029309 -0.668553762428\n",
      "\n",
      "num = \n",
      "1215\n",
      "SENSOR 4 1.84304791933 -2.65607644439\n",
      "\n",
      "num = \n",
      "1216\n",
      "SENSOR 5 4.63624935942 -2.13548023805\n",
      "\n",
      "num = \n",
      "1217\n",
      "SENSOR 7 4.52016763586 -0.991018307807\n",
      "\n",
      "num = \n",
      "1218\n",
      "ODOMETRY 0.000189591679111 0.100086348204 -0.000323351005088\n",
      "\n",
      "sqP = \n",
      "[[1.6148041563930966, 0.0, 0.0], [0.0014635366638763975, 0.25465191214829197, 0.0], [0.04530362694252848, 0.030100935978424913, 0.30164870895287366]]\n",
      "sigma = \n",
      "[[ 1.93452999  7.29738292  1.54455119]\n",
      " [ 5.1641383   7.29738292  1.54455119]\n",
      " [ 1.93745706  7.80668674  1.54455119]\n",
      " [ 2.02513724  7.35758479  2.14784861]\n",
      " [-1.29507833  7.29738292  1.54455119]\n",
      " [ 1.93160291  6.78807909  1.54455119]\n",
      " [ 1.84392273  7.23718105  0.94125377]]\n",
      "sqP = \n",
      "[[1.4333673900378483, 0.0, 0.0], [-0.02794409152280577, 0.5153898090256865, 0.0], [0.08612840497913475, 0.014909692869688832, 0.5374941886590399]]\n",
      "m = \n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[2.06771385 7.21259842 1.51373287]\n",
      "P = \n",
      "[[ 0.65362261 -0.02340995  0.07768439]\n",
      " [-0.02340995  0.0665794   0.0050745 ]\n",
      " [ 0.07768439  0.0050745   0.09536838]]\n",
      "num = \n",
      "1219\n",
      "SENSOR 8 2.93905607509 -0.585176239548\n",
      "\n",
      "num = \n",
      "1220\n",
      "SENSOR 4 2.0438034489 -2.72800462462\n",
      "\n",
      "num = \n",
      "1221\n",
      "SENSOR 5 4.52648542497 -2.21078238898\n",
      "\n",
      "num = \n",
      "1222\n",
      "SENSOR 7 4.18292097158 -1.01609850061\n",
      "\n",
      "num = \n",
      "1223\n",
      "ODOMETRY -0.000473674854813 0.10002979275 -7.07593512027e-05\n",
      "\n",
      "sqP = \n",
      "[[0.8084693013948792, 0.0, 0.0], [-0.028955886557706458, 0.2563999858789784, 0.0], [0.09608823185214646, 0.03064280672182075, 0.29188431547956034]]\n",
      "sigma = \n",
      "[[2.06771385 7.21259842 1.51373287]\n",
      " [3.68465245 7.21259842 1.51373287]\n",
      " [2.00980208 7.7253984  1.51373287]\n",
      " [2.25989031 7.27388404 2.0975015 ]\n",
      " [0.45077525 7.21259842 1.51373287]\n",
      " [2.12562562 6.69979845 1.51373287]\n",
      " [1.87553739 7.15131281 0.92996424]]\n",
      "sqP = \n",
      "[[1.625350642331158, 0.0, 0.0], [-0.0006695957155534056, 0.5195570226998882, 0.0], [0.06128245848416355, 0.01344603584781456, 0.5298664360687482]]\n",
      "m = \n",
      "[1.9121584  7.13544796 1.56765104]\n",
      "P = \n",
      "[[2.60760804 0.00338888 0.07283151]\n",
      " [0.00338888 0.06674674 0.00679648]\n",
      " [0.07283151 0.00679648 0.09072932]]\n",
      "num = \n",
      "1224\n",
      "SENSOR 8 2.78937751626 -0.681683822875\n",
      "\n",
      "num = \n",
      "1225\n",
      "SENSOR 4 1.99021011507 -2.71901554183\n",
      "\n",
      "num = \n",
      "1226\n",
      "SENSOR 5 4.86030610026 -2.19809066259\n",
      "\n",
      "num = \n",
      "1227\n",
      "SENSOR 7 4.08317192213 -1.06263343315\n",
      "\n",
      "num = \n",
      "1228\n",
      "ODOMETRY -0.000361318872101 0.0998516170993 -0.000310786635761\n",
      "\n",
      "sqP = \n",
      "[[1.6148089805711696, 0.0, 0.0], [0.0020986257734804076, 0.25834538569446613, 0.0], [0.04510224723918461, 0.02594133048496582, 0.2966852788931888]]\n",
      "sigma = \n",
      "[[ 1.9121584   7.13544796  1.56765104]\n",
      " [ 5.14177636  7.13544796  1.56765104]\n",
      " [ 1.91635565  7.65213873  1.56765104]\n",
      " [ 2.0023629   7.18733062  2.1610216 ]\n",
      " [-1.31745956  7.13544796  1.56765104]\n",
      " [ 1.90796115  6.61875719  1.56765104]\n",
      " [ 1.82195391  7.0835653   0.97428048]]\n",
      "sqP = \n",
      "[[1.4333619765547174, 0.0, 0.0], [-0.026226975804755484, 0.5169860415220836, 0.0], [0.08661535790945031, 0.01220799267535089, 0.5348947865913062]]\n",
      "m = \n",
      "[2.07072357 7.00019003 1.57281149]\n",
      "P = \n",
      "[[ 0.65364377 -0.02167242  0.07797851]\n",
      " [-0.02167242  0.06679322  0.00511576]\n",
      " [ 0.07797851  0.00511576  0.09504135]]\n",
      "num = \n",
      "1229\n",
      "SENSOR 8 2.73109069077 -0.664414233072\n",
      "\n",
      "num = \n",
      "1230\n",
      "SENSOR 4 2.02095934862 -2.77281077247\n",
      "\n",
      "num = \n",
      "1231\n",
      "SENSOR 5 4.87996286947 -2.20004470209\n",
      "\n",
      "num = \n",
      "1232\n",
      "SENSOR 7 4.22842676961 -1.08754115393\n",
      "\n",
      "num = \n",
      "1233\n",
      "ODOMETRY -0.000269192638065 0.100069154562 0.000122327434553\n",
      "\n",
      "sqP = \n",
      "[[0.8084823872693693, 0.0, 0.0], [-0.02680630212979043, 0.2570498801063721, 0.0], [0.09645047962478938, 0.029960091707184004, 0.2912748642125354]]\n",
      "sigma = \n",
      "[[2.07072357 7.00019003 1.57281149]\n",
      " [3.68768834 7.00019003 1.57281149]\n",
      " [2.01711096 7.51428979 1.57281149]\n",
      " [2.26362452 7.06011021 2.15536121]\n",
      " [0.45375879 7.00019003 1.57281149]\n",
      " [2.12433617 6.48609026 1.57281149]\n",
      " [1.87782261 6.94026984 0.99026176]]\n",
      "sqP = \n",
      "[[1.6253290868168542, 0.0, 0.0], [-0.00048647705400455134, 0.5197424190155014, 0.0], [0.06121707898210116, 0.012966314305394477, 0.5295325434963788]]\n",
      "m = \n",
      "[1.9067669  6.94903011 1.47686029]\n",
      "P = \n",
      "[[2.60763195 0.00361663 0.07323392]\n",
      " [0.00361663 0.0666814  0.00697913]\n",
      " [0.07323392 0.00697913 0.09114039]]\n",
      "num = \n",
      "1234\n",
      "SENSOR 8 2.66996787215 -0.741197858346\n",
      "\n",
      "num = \n",
      "1235\n",
      "SENSOR 1 4.92781804914 0.165464879583\n",
      "\n",
      "num = \n",
      "1236\n",
      "SENSOR 4 2.28452084059 -2.79103144813\n",
      "\n",
      "num = \n",
      "1237\n",
      "SENSOR 5 4.76222916544 -2.3384152959\n",
      "\n",
      "num = \n",
      "1238\n",
      "SENSOR 7 4.19121425588 -1.02506412666\n",
      "\n",
      "num = \n",
      "1239\n",
      "ODOMETRY 0.000939233063028 0.099978524037 0.000685447903704\n",
      "\n",
      "sqP = \n",
      "[[1.614816381258497, 0.0, 0.0], [0.0022396526821414665, 0.25821770329769894, 0.0], [0.04535123536025106, 0.026634717022449485, 0.29727805753344294]]\n",
      "sigma = \n",
      "[[ 1.9067669   6.94903011  1.47686029]\n",
      " [ 5.13639967  6.94903011  1.47686029]\n",
      " [ 1.91124621  7.46546552  1.47686029]\n",
      " [ 1.99746937  7.00229955  2.0714164 ]\n",
      " [-1.32286586  6.94903011  1.47686029]\n",
      " [ 1.9022876   6.43259471  1.47686029]\n",
      " [ 1.81606443  6.89576068  0.88230417]]\n",
      "sqP = \n",
      "[[1.4333702735134668, 0.0, 0.0], [-0.025785828119545107, 0.5169429968037783, 0.0], [0.08685564402159429, 0.012603460846488041, 0.5352087574400234]]\n",
      "m = \n",
      "[2.10205054 6.85905953 1.53446935]\n",
      "P = \n",
      "[[ 6.51936944e-01 -2.21036261e-02  7.82987075e-02]\n",
      " [-2.21036261e-02  5.12728678e-02 -5.72616634e-05]\n",
      " [ 7.82987075e-02 -5.72616634e-05  9.05974955e-02]]\n",
      "num = \n",
      "1240\n",
      "SENSOR 8 2.51553389791 -0.634533883027\n",
      "\n",
      "num = \n",
      "1241\n",
      "SENSOR 1 4.82706330783 0.0213662625167\n",
      "\n",
      "num = \n",
      "1242\n",
      "SENSOR 4 2.37505836382 -2.78320616069\n",
      "\n",
      "num = \n",
      "1243\n",
      "SENSOR 5 4.99415607057 -2.22355221099\n",
      "\n",
      "num = \n",
      "1244\n",
      "SENSOR 7 3.97703512937 -1.08364781079\n",
      "\n",
      "num = \n",
      "1245\n",
      "ODOMETRY -6.77419651127e-05 0.100140978268 -0.000135987996048\n",
      "\n",
      "sqP = \n",
      "[[0.8074261226715753, 0.0, 0.0], [-0.027375416151192215, 0.22477422972272731, 0.0], [0.09697321562669045, 0.01155568622410763, 0.28471065497167314]]\n",
      "sigma = \n",
      "[[2.10205054 6.85905953 1.53446935]\n",
      " [3.71690279 6.85905953 1.53446935]\n",
      " [2.04729971 7.30860799 1.53446935]\n",
      " [2.29599698 6.8821709  2.10389066]\n",
      " [0.4871983  6.85905953 1.53446935]\n",
      " [2.15680138 6.40951107 1.53446935]\n",
      " [1.90810411 6.83594815 0.96504804]]\n",
      "sqP = \n",
      "[[1.6258653424272156, 0.0, 0.0], [-0.0024906155423627405, 0.5039487712440425, 0.0], [0.060940843050838675, 0.0031472205176542017, 0.526040331913193]]\n",
      "m = \n",
      "[2.05490144 6.70518593 1.53429571]\n",
      "P = \n",
      "[[2.60213205e+00 9.78431765e-05 7.21326653e-02]\n",
      " [9.78431765e-05 4.99543971e-02 1.55504619e-03]\n",
      " [7.21326653e-02 1.55504619e-03 8.71792233e-02]]\n",
      "num = \n",
      "1246\n",
      "SENSOR 8 2.23340888122 -0.783411064361\n",
      "\n",
      "num = \n",
      "1247\n",
      "SENSOR 1 4.76673370222 -0.00223170351281\n",
      "\n",
      "num = \n",
      "1248\n",
      "SENSOR 4 2.44121554947 -2.76116600902\n",
      "\n",
      "num = \n",
      "1249\n",
      "SENSOR 5 4.814470924 -2.31902709448\n",
      "\n",
      "num = \n",
      "1250\n",
      "SENSOR 7 4.04347080517 -1.14617801134\n",
      "\n",
      "num = \n",
      "1251\n",
      "ODOMETRY 0.00048886726123 0.100041297815 0.000679022990554\n",
      "\n",
      "sqP = \n",
      "[[1.6131125350426123, 0.0, 0.0], [6.065489812778458e-05, 0.22350479498927267, 0.0], [0.04471644957581559, 0.006945416641639267, 0.29177289740335016]]\n",
      "sigma = \n",
      "[[ 2.05490144  6.70518593  1.53429571]\n",
      " [ 5.28112651  6.70518593  1.53429571]\n",
      " [ 2.05502275  7.15219552  1.53429571]\n",
      " [ 2.14433434  6.71907676  2.11784151]\n",
      " [-1.17132363  6.70518593  1.53429571]\n",
      " [ 2.05478013  6.25817634  1.53429571]\n",
      " [ 1.96546854  6.69129509  0.95074992]]\n",
      "sqP = \n",
      "[[1.4332972654778917, 0.0, 0.0], [-0.037996538405014026, 0.5001519622964177, 0.0], [0.08177232532382857, 0.001792261548490381, 0.53207032593739]]\n",
      "m = \n",
      "[-0.29813701  6.60034445  1.63011532]\n",
      "P = \n",
      "[[ 1.88529454e+00 -4.87466958e-02  9.75510502e-02]\n",
      " [-4.87466958e-02  5.16284910e-02 -1.55112064e-03]\n",
      " [ 9.75510502e-02 -1.55112064e-03  8.89505119e-02]]\n",
      "num = \n",
      "1252\n",
      "SENSOR 8 2.30974435417 -0.73023844975\n",
      "\n",
      "num = \n",
      "1253\n",
      "SENSOR 1 4.628923258 0.0668774699138\n",
      "\n",
      "num = \n",
      "1254\n",
      "SENSOR 4 2.33202866839 -2.80658070702\n",
      "\n",
      "num = \n",
      "1255\n",
      "SENSOR 7 4.1171835927 -1.18735408115\n",
      "\n",
      "num = \n",
      "1256\n",
      "ODOMETRY -0.000763316825852 0.0998626212511 2.71380464145e-05\n",
      "\n",
      "sqP = \n",
      "[[1.3730602819335433, 0.0, 0.0], [-0.0355022254990766, 0.22442834711521856, 0.0], [0.0710464438079116, 0.004327377703402747, 0.28962767223846125]]\n",
      "sigma = \n",
      "[[-0.29813701  6.60034445  1.63011532]\n",
      " [ 2.44798355  6.60034445  1.63011532]\n",
      " [-0.36914146  7.04920114  1.63011532]\n",
      " [-0.15604412  6.6089992   2.20937066]\n",
      " [-3.04425758  6.60034445  1.63011532]\n",
      " [-0.22713256  6.15148775  1.63011532]\n",
      " [-0.4402299   6.59168969  1.05085998]]\n",
      "sqP = \n",
      "[[1.4462373743672094, 0.0, 0.0], [0.00019637475980939213, 0.5062496243663348, 0.0], [0.03213459010844439, -0.001379742202384831, 0.5364358317425415]]\n",
      "m = \n",
      "[-0.55381635  6.69153541  1.52504269]\n",
      "P = \n",
      "[[2.00204420e+00 8.34413849e-04 4.67462335e-02]\n",
      " [8.34413849e-04 6.03878995e-02 1.38415732e-02]\n",
      " [4.67462335e-02 1.38415732e-02 8.65115233e-02]]\n",
      "num = \n",
      "1257\n",
      "SENSOR 8 2.30957175125 -0.879774630027\n",
      "\n",
      "num = \n",
      "1258\n",
      "SENSOR 1 4.47241468075 0.112481627522\n",
      "\n",
      "num = \n",
      "1259\n",
      "SENSOR 4 2.67200407606 -2.88213261922\n",
      "\n",
      "num = \n",
      "1260\n",
      "SENSOR 7 4.13385546866 -1.15907658911\n",
      "\n",
      "num = \n",
      "1261\n",
      "ODOMETRY 0.00049292915547 0.0998636041901 -0.000379476978871\n",
      "\n",
      "sqP = \n",
      "[[1.4149361112974386, 0.0, 0.0], [0.0005897183925512906, 0.2457387875589652, 0.0], [0.033037699077156917, 0.056247084124376484, 0.2868035899746018]]\n",
      "sigma = \n",
      "[[-0.55381635  6.69153541  1.52504269]\n",
      " [ 2.27605587  6.69153541  1.52504269]\n",
      " [-0.55263691  7.18301299  1.52504269]\n",
      " [-0.48774095  6.80402958  2.09864987]\n",
      " [-3.38368857  6.69153541  1.52504269]\n",
      " [-0.55499579  6.20005784  1.52504269]\n",
      " [-0.61989175  6.57904124  0.95143551]]\n",
      "sqP = \n",
      "[[1.4408105598264, 0.0, 0.0], [-0.06716007472993725, 0.5142602633675463, 0.0], [0.04717848386014434, 0.03284644973381561, 0.5317051475187342]]\n",
      "m = \n",
      "[2.13791086 6.5142692  1.31896362]\n",
      "P = \n",
      "[[ 0.67363768 -0.06673361  0.01592283]\n",
      " [-0.06673361  0.06523183  0.01452136]\n",
      " [ 0.01592283  0.01452136  0.1127185 ]]\n",
      "num = \n",
      "1262\n",
      "SENSOR 8 2.36124708193 -0.870145768712\n",
      "\n",
      "num = \n",
      "1263\n",
      "SENSOR 1 4.38735357247 0.0955180040226\n",
      "\n",
      "num = \n",
      "1264\n",
      "SENSOR 4 2.65985478012 -2.84310942138\n",
      "\n",
      "num = \n",
      "1265\n",
      "SENSOR 7 3.81181745959 -1.20782384771\n",
      "\n",
      "num = \n",
      "1266\n",
      "ODOMETRY 0.00033110801736 0.100044850743 -0.000577153724803\n",
      "\n",
      "sqP = \n",
      "[[0.820754334167936, 0.0, 0.0], [-0.08130765416856936, 0.24211751365867218, 0.0], [0.019400232886220795, 0.06649144863033074, 0.32851335129266807]]\n",
      "sigma = \n",
      "[[2.13791086 6.5142692  1.31896362]\n",
      " [3.77941953 6.5142692  1.31896362]\n",
      " [1.97529555 6.99850423 1.31896362]\n",
      " [2.17671133 6.6472521  1.97599032]\n",
      " [0.49640219 6.5142692  1.31896362]\n",
      " [2.30052617 6.03003417 1.31896362]\n",
      " [2.0991104  6.3812863  0.66193691]]\n",
      "sqP = \n",
      "[[1.618672641262127, 0.0, 0.0], [-0.013094090468442848, 0.5180551560101344, 0.0], [0.04877927207761127, 0.044630511410636876, 0.5523707615568519]]\n",
      "m = \n",
      "[1.76725837 6.39104133 1.46139403]\n",
      "P = \n",
      "[[ 2.59939761 -0.01457382  0.05875099]\n",
      " [-0.01457382  0.0630402   0.01501149]\n",
      " [ 0.05875099  0.01501149  0.10713247]]\n",
      "num = \n",
      "1267\n",
      "SENSOR 8 2.41743004232 -0.864072811384\n",
      "\n",
      "num = \n",
      "1268\n",
      "SENSOR 1 4.40194504238 0.0658008097918\n",
      "\n",
      "num = \n",
      "1269\n",
      "SENSOR 4 2.94567913337 -2.88562727445\n",
      "\n",
      "num = \n",
      "1270\n",
      "SENSOR 7 3.82544464355 -1.2516790519\n",
      "\n",
      "num = \n",
      "1271\n",
      "ODOMETRY -0.000308672136194 0.0998384881725 2.64214654053e-05\n",
      "\n",
      "sqP = \n",
      "[[1.6122647466387412, 0.0, 0.0], [-0.009039347521416946, 0.2509153146155555, 0.0], [0.036440039732232095, 0.06113969950434326, 0.3194785240208863]]\n",
      "sigma = \n",
      "[[ 1.76725837  6.39104133  1.46139403]\n",
      " [ 4.99178786  6.39104133  1.46139403]\n",
      " [ 1.74917967  6.89287196  1.46139403]\n",
      " [ 1.84013844  6.51332073  2.10035108]\n",
      " [-1.45727113  6.39104133  1.46139403]\n",
      " [ 1.78533706  5.8892107   1.46139403]\n",
      " [ 1.69437829  6.26876193  0.82243698]]\n",
      "sqP = \n",
      "[[1.433066656282219, 0.0, 0.0], [-0.016052657900352443, 0.5164909567144196, 0.0], [0.08860897901047808, 0.035601201765687034, 0.54743206269318]]\n",
      "m = \n",
      "[-0.23835488  6.29217731  1.48251826]\n",
      "P = \n",
      "[[ 1.93804874 -0.02151768  0.10378795]\n",
      " [-0.02151768  0.06336773  0.0142084 ]\n",
      " [ 0.10378795  0.0142084   0.11168372]]\n",
      "num = \n",
      "1272\n",
      "SENSOR 8 1.919908976 -0.934813972655\n",
      "\n",
      "num = \n",
      "1273\n",
      "SENSOR 1 4.19345581376 0.0757952107782\n",
      "\n",
      "num = \n",
      "1274\n",
      "SENSOR 4 2.94440761406 -2.83450142815\n",
      "\n",
      "num = \n",
      "1275\n",
      "SENSOR 7 3.92107607518 -1.15208824306\n",
      "\n",
      "num = \n",
      "1276\n",
      "ODOMETRY 5.25016195751e-05 0.0999941288634 0.000406623957959\n",
      "\n",
      "sqP = \n",
      "[[1.3921381906247505, 0.0, 0.0], [-0.015456567271759664, 0.25125451063770937, 0.0], [0.07455291086523341, 0.06113613764580391, 0.31998117929886266]]\n",
      "sigma = \n",
      "[[-0.23835488  6.29217731  1.48251826]\n",
      " [ 2.5459215   6.29217731  1.48251826]\n",
      " [-0.26926801  6.79468633  1.48251826]\n",
      " [-0.08924906  6.41444958  2.12248062]\n",
      " [-3.02263126  6.29217731  1.48251826]\n",
      " [-0.20744174  5.78966829  1.48251826]\n",
      " [-0.3874607   6.16990503  0.8425559 ]]\n",
      "sqP = \n",
      "[[1.4641877556468843, 0.0, 0.0], [0.0044781396442512104, 0.5229634741966179, 0.0], [0.03280549169655281, 0.0354124299620384, 0.5526311771808692]]\n",
      "m = \n",
      "[-0.43084446  6.17418374  1.37772587]\n",
      "P = \n",
      "[[2.06945893e+00 8.94706991e-04 3.10438549e-02]\n",
      " [8.94706991e-04 6.40733717e-02 1.38469055e-02]\n",
      " [3.10438549e-02 1.38469055e-02 1.03074878e-01]]\n",
      "num = \n",
      "1277\n",
      "SENSOR 8 1.9832859543 -0.98442650848\n",
      "\n",
      "num = \n",
      "1278\n",
      "SENSOR 1 4.09215025882 0.0717523426929\n",
      "\n",
      "num = \n",
      "1279\n",
      "SENSOR 4 3.05636372159 -2.93752404177\n",
      "\n",
      "num = \n",
      "1280\n",
      "SENSOR 7 3.89565529492 -1.28776086607\n",
      "\n",
      "num = \n",
      "1281\n",
      "ODOMETRY -0.000273197315602 0.0998069485111 -0.000285894067554\n",
      "\n",
      "sqP = \n",
      "[[1.43856140952347, 0.0, 0.0], [0.0006219456359726009, 0.2531264206908873, 0.0], [0.02157979126953919, 0.054650494406479004, 0.3156303443262812]]\n",
      "sigma = \n",
      "[[-0.43084446  6.17418374  1.37772587]\n",
      " [ 2.44627836  6.17418374  1.37772587]\n",
      " [-0.42960056  6.68043658  1.37772587]\n",
      " [-0.38768487  6.28348473  2.00898656]\n",
      " [-3.30796727  6.17418374  1.37772587]\n",
      " [-0.43208835  5.6679309   1.37772587]\n",
      " [-0.47400404  6.06488275  0.74646518]]\n",
      "sqP = \n",
      "[[1.4382291099674094, 0.0, 0.0], [-0.07267812010926057, 0.5175865925940265, 0.0], [0.037277630139673466, 0.03416711670346184, 0.5474576980173075]]\n",
      "m = \n",
      "[2.19047299 6.06038784 1.33155782]\n",
      "P = \n",
      "[[ 0.67010726 -0.07056715  0.01328581]\n",
      " [-0.07056715  0.07072205  0.00978136]\n",
      " [ 0.01328581  0.00978136  0.09811663]]\n",
      "num = \n",
      "1282\n",
      "SENSOR 8 2.12296121839 -1.02645711564\n",
      "\n",
      "num = \n",
      "1283\n",
      "SENSOR 1 4.1786687521 0.0497541354582\n",
      "\n",
      "num = \n",
      "1284\n",
      "SENSOR 4 3.22093748114 -2.82397420009\n",
      "\n",
      "num = \n",
      "1285\n",
      "SENSOR 7 3.97449556221 -1.29780080623\n",
      "\n",
      "num = \n",
      "1286\n",
      "ODOMETRY -0.000265608833175 0.100026175106 0.000137251905213\n",
      "\n",
      "sqP = \n",
      "[[0.8186007946256114, 0.0, 0.0], [-0.08620459221290355, 0.2515766572868935, 0.0], [0.016229901692324415, 0.04444152073034466, 0.30964200465255265]]\n",
      "sigma = \n",
      "[[2.19047299 6.06038784 1.33155782]\n",
      " [3.82767457 6.06038784 1.33155782]\n",
      " [2.0180638  6.56354116 1.33155782]\n",
      " [2.22293279 6.14927089 1.95084183]\n",
      " [0.5532714  6.06038784 1.33155782]\n",
      " [2.36288217 5.55723453 1.33155782]\n",
      " [2.15801318 5.9715048  0.71227381]]\n",
      "sqP = \n",
      "[[1.6198899917115435, 0.0, 0.0], [-0.016861078612448878, 0.5203603509854915, 0.0], [0.047612231431403154, 0.029935846664858642, 0.5425761205913775]]\n",
      "m = \n",
      "[1.80377753 5.97629956 1.31194228]\n",
      "P = \n",
      "[[ 2.60236684 -0.01843845  0.05758487]\n",
      " [-0.01843845  0.06719195  0.01006953]\n",
      " [ 0.05758487  0.01006953  0.09399509]]\n",
      "num = \n",
      "1287\n",
      "SENSOR 8 2.05703216981 -1.00685667222\n",
      "\n",
      "num = \n",
      "1288\n",
      "SENSOR 1 3.94149875624 0.140569733461\n",
      "\n",
      "num = \n",
      "1289\n",
      "SENSOR 4 3.13725363665 -2.87162377088\n",
      "\n",
      "num = \n",
      "1290\n",
      "SENSOR 7 3.76919105935 -1.28829235854\n",
      "\n",
      "num = \n",
      "1291\n",
      "ODOMETRY 0.000720910278101 0.100023575271 -0.000251711733041\n",
      "\n",
      "sqP = \n",
      "[[1.6131853090247485, 0.0, 0.0], [-0.011429842477648234, 0.25896197688690853, 0.0], [0.035696377768622195, 0.04045972819893757, 0.3018010422984156]]\n",
      "sigma = \n",
      "[[ 1.80377753  5.97629956  1.31194228]\n",
      " [ 5.03014814  5.97629956  1.31194228]\n",
      " [ 1.78091784  6.49422351  1.31194228]\n",
      " [ 1.87517028  6.05721901  1.91554436]\n",
      " [-1.42259309  5.97629956  1.31194228]\n",
      " [ 1.82663721  5.4583756   1.31194228]\n",
      " [ 1.73238477  5.8953801   0.70834019]]\n",
      "sqP = \n",
      "[[1.4330919453069302, 0.0, 0.0], [-0.019951643162817185, 0.5186742859274357, 0.0], [0.0873542383295643, 0.021732030985505937, 0.5381065070794521]]\n",
      "m = \n",
      "[-0.12239783  5.88519485  1.43796163]\n",
      "P = \n",
      "[[ 1.93762521 -0.02735407  0.09937637]\n",
      " [-0.02735407  0.06809862  0.00832689]\n",
      " [ 0.09937637  0.00832689  0.09614441]]\n",
      "num = \n",
      "1292\n",
      "SENSOR 8 1.81579376561 -1.08266616382\n",
      "\n",
      "num = \n",
      "1293\n",
      "SENSOR 1 3.67093999662 0.0393555510994\n",
      "\n",
      "num = \n",
      "1294\n",
      "SENSOR 4 3.34596406803 -2.97433102412\n",
      "\n",
      "num = \n",
      "1295\n",
      "SENSOR 7 3.90187420479 -1.31346748714\n",
      "\n",
      "num = \n",
      "1296\n",
      "ODOMETRY 0.000105339136838 0.099882486807 0.000711003741797\n",
      "\n",
      "sqP = \n",
      "[[1.3919860668210264, 0.0, 0.0], [-0.019651109136553276, 0.2602161762522624, 0.0], [0.07139178401376584, 0.037391288195690466, 0.299415287596075]]\n",
      "sigma = \n",
      "[[-0.12239783  5.88519485  1.43796163]\n",
      " [ 2.66157431  5.88519485  1.43796163]\n",
      " [-0.16170004  6.4056272   1.43796163]\n",
      " [ 0.02038574  5.95997743  2.03679221]\n",
      " [-2.90636996  5.88519485  1.43796163]\n",
      " [-0.08309561  5.3647625   1.43796163]\n",
      " [-0.26518139  5.81041228  0.83913106]]\n",
      "sqP = \n",
      "[[1.463935846660924, 0.0, 0.0], [0.0003962256070215095, 0.5253217140016493, 0.0], [0.031495222379101045, 0.019792022905433936, 0.5412531039581278]]\n",
      "m = \n",
      "[-0.27578337  5.74305686  1.30037716]\n",
      "P = \n",
      "[[ 2.06868961 -0.00389719  0.02947916]\n",
      " [-0.00389719  0.06818678  0.00854809]\n",
      " [ 0.02947916  0.00854809  0.09044863]]\n",
      "num = \n",
      "1297\n",
      "SENSOR 8 1.81140030994 -1.14228995915\n",
      "\n",
      "num = \n",
      "1298\n",
      "SENSOR 1 3.8261635831 0.054006173831\n",
      "\n",
      "num = \n",
      "1299\n",
      "SENSOR 4 3.4796093187 -2.98658709002\n",
      "\n",
      "num = \n",
      "1300\n",
      "SENSOR 7 3.82419437586 -1.32957085243\n",
      "\n",
      "num = \n",
      "1301\n",
      "ODOMETRY -7.64579991343e-05 0.0997922673878 -0.000288040106145\n",
      "\n",
      "sqP = \n",
      "[[1.4382939934142904, 0.0, 0.0], [-0.0027095928351130233, 0.26111193251889153, 0.0], [0.020495922594440507, 0.03294996538260967, 0.29823287825411565]]\n",
      "sigma = \n",
      "[[-0.27578337  5.74305686  1.30037716]\n",
      " [ 2.60080462  5.74305686  1.30037716]\n",
      " [-0.28120255  6.26528072  1.30037716]\n",
      " [-0.23479152  5.80895679  1.89684291]\n",
      " [-3.15237136  5.74305686  1.30037716]\n",
      " [-0.27036418  5.22083299  1.30037716]\n",
      " [-0.31677521  5.67715693  0.7039114 ]]\n",
      "sqP = \n",
      "[[1.438240285311147, 0.0, 0.0], [-0.07768380588543214, 0.5195458794760731, 0.0], [0.02499744223280694, 0.019413472679399427, 0.5383429639781709]]\n",
      "m = \n",
      "[-0.87784647  5.56633511  1.35566703]\n",
      "P = \n",
      "[[ 1.96159796 -0.09316641  0.02678913]\n",
      " [-0.09316641  0.07454955  0.00517352]\n",
      " [ 0.02678913  0.00517352  0.08505659]]\n",
      "num = \n",
      "1302\n",
      "SENSOR 8 1.97927232141 -1.27560024901\n",
      "\n",
      "num = \n",
      "1303\n",
      "SENSOR 1 3.72258806814 0.161210114162\n",
      "\n",
      "num = \n",
      "1304\n",
      "SENSOR 4 3.3133245266 -2.89891758969\n",
      "\n",
      "num = \n",
      "1305\n",
      "SENSOR 7 3.81183963458 -1.32175712764\n",
      "\n",
      "num = \n",
      "1306\n",
      "ODOMETRY 0.000385928532719 0.0996806600309 -0.200347125045\n",
      "\n",
      "sqP = \n",
      "[[1.4005705832900985, 0.0, 0.0], [-0.06652032271099043, 0.2648104970938767, 0.0], [0.01912729654127741, 0.02434146914073303, 0.289996953869525]]\n",
      "sigma = \n",
      "[[-0.87784647  5.56633511  1.35566703]\n",
      " [ 1.9232947   5.56633511  1.35566703]\n",
      " [-1.01088712  6.0959561   1.35566703]\n",
      " [-0.83959188  5.61501805  1.93566093]\n",
      " [-3.67898764  5.56633511  1.35566703]\n",
      " [-0.74480583  5.03671412  1.35566703]\n",
      " [-0.91610106  5.51765217  0.77567312]]\n",
      "sqP = \n",
      "[[1.4437168821353334, 0.0, 0.0], [-0.06357541504545978, 0.516824562534853, 0.0], [0.06355525120266915, 0.013821832964688877, 0.5337277430735704]]\n",
      "m = \n",
      "[-0.24457132  5.68452798  1.25453216]\n",
      "P = \n",
      "[[ 1.98178344e+00 -7.76122355e-02  6.53522822e-02]\n",
      " [-7.76122355e-02  7.39662570e-02 -9.12301688e-04]\n",
      " [ 6.53522822e-02 -9.12301688e-04  8.52771623e-02]]\n",
      "num = \n",
      "1307\n",
      "SENSOR 8 1.71109074699 -1.02714696631\n",
      "\n",
      "num = \n",
      "1308\n",
      "SENSOR 1 3.47474230224 0.283685656426\n",
      "\n",
      "num = \n",
      "1309\n",
      "SENSOR 4 3.49665186449 -2.71608754934\n",
      "\n",
      "num = \n",
      "1310\n",
      "SENSOR 7 3.86100523492 -1.24660856692\n",
      "\n",
      "num = \n",
      "1311\n",
      "ODOMETRY 1.18794066805e-06 0.0997733761035 -0.199977592699\n",
      "\n",
      "sqP = \n",
      "[[1.4077583035390775, 0.0, 0.0], [-0.055131790253775716, 0.26632075162083974, 0.0], [0.046422942057023533, 0.006184565814496431, 0.28824264757505985]]\n",
      "sigma = \n",
      "[[-0.24457132  5.68452798  1.25453216]\n",
      " [ 2.57094529  5.68452798  1.25453216]\n",
      " [-0.3548349   6.21716948  1.25453216]\n",
      " [-0.15172543  5.69689711  1.83101746]\n",
      " [-3.06008793  5.68452798  1.25453216]\n",
      " [-0.13430774  5.15188647  1.25453216]\n",
      " [-0.3374172   5.67215884  0.67804687]]\n",
      "sqP = \n",
      "[[1.4430269665107565, 0.0, 0.0], [-0.08707421315855267, 0.5189765894901577, 0.0], [0.026992688478293014, 0.0028630965429842573, 0.5344452478108263]]\n",
      "m = \n",
      "[2.04117335 5.53094109 1.28876896]\n",
      "P = \n",
      "[[ 0.66852892 -0.08252027  0.00938843]\n",
      " [-0.08252027  0.0777848   0.00295328]\n",
      " [ 0.00938843  0.00295328  0.08265469]]\n",
      "num = \n",
      "1312\n",
      "SENSOR 8 1.7913022851 -0.988844108691\n",
      "\n",
      "num = \n",
      "1313\n",
      "SENSOR 1 3.3603196986 0.438921773545\n",
      "\n",
      "num = \n",
      "1314\n",
      "SENSOR 4 3.59460143268 -2.4995361102\n",
      "\n",
      "num = \n",
      "1315\n",
      "SENSOR 7 3.6769689176 -1.0092059111\n",
      "\n",
      "num = \n",
      "1316\n",
      "ODOMETRY -0.000816556751862 0.0997465372851 -0.202314638633\n",
      "\n",
      "sqP = \n",
      "[[0.8176361760368644, 0.0, 0.0], [-0.10092542054754666, 0.25999779920051863, 0.0], [0.011482400698929213, 0.01581609383747389, 0.28683216461881056]]\n",
      "sigma = \n",
      "[[2.04117335 5.53094109 1.28876896]\n",
      " [3.6764457  5.53094109 1.28876896]\n",
      " [1.83932251 6.05093669 1.28876896]\n",
      " [2.06413815 5.56257328 1.86243329]\n",
      " [0.40590099 5.53094109 1.28876896]\n",
      " [2.24302419 5.01094549 1.28876896]\n",
      " [2.01820854 5.4993089  0.71510463]]\n",
      "sqP = \n",
      "[[1.6211473788439552, 0.0, 0.0], [-0.01339662246313832, 0.5241677944912555, 0.0], [0.04640444612882624, 0.01189348808436801, 0.5309136157935619]]\n",
      "m = \n",
      "[1.46714849 5.36538679 1.41036436]\n",
      "P = \n",
      "[[ 2.60558298e+00 -1.36842630e-02  5.59887272e-02]\n",
      " [-1.36842630e-02  7.14543235e-02  2.11129784e-03]\n",
      " [ 5.59887272e-02  2.11129784e-03  8.19160324e-02]]\n",
      "num = \n",
      "1317\n",
      "SENSOR 8 1.72700243897 -0.727469713931\n",
      "\n",
      "num = \n",
      "1318\n",
      "SENSOR 1 3.41603446384 0.795914407086\n",
      "\n",
      "num = \n",
      "1319\n",
      "SENSOR 4 3.74498201327 -2.35790495186\n",
      "\n",
      "num = \n",
      "1320\n",
      "SENSOR 7 3.65858016772 -0.852210047465\n",
      "\n",
      "num = \n",
      "1321\n",
      "ODOMETRY -0.00017107666944 0.100202752156 -0.199982427928\n",
      "\n",
      "sqP = \n",
      "[[1.6141818300385227, 0.0, 0.0], [-0.008477522619841638, 0.26717495222959714, 0.0], [0.034685514450515334, 0.009002883901072485, 0.2839575594797613]]\n",
      "sigma = \n",
      "[[ 1.46714849  5.36538679  1.41036436]\n",
      " [ 4.69551215  5.36538679  1.41036436]\n",
      " [ 1.45019345  5.8997367   1.41036436]\n",
      " [ 1.53651952  5.38339256  1.97827948]\n",
      " [-1.76121517  5.36538679  1.41036436]\n",
      " [ 1.48410354  4.83103689  1.41036436]\n",
      " [ 1.39777746  5.34738102  0.84244924]]\n",
      "sqP = \n",
      "[[1.4330764391331066, 0.0, 0.0], [0.007210053382323485, 0.5214506180945705, 0.0], [0.08823250915222511, 0.003242341724208073, 0.5295171089168067]]\n",
      "m = \n",
      "[-0.6530871   5.29433915  1.50439952]\n",
      "P = \n",
      "[[1.93691433e+00 1.57223226e-03 1.00039930e-01]\n",
      " [1.57223226e-03 7.18812489e-02 1.07279849e-03]\n",
      " [1.00039930e-01 1.07279849e-03 8.62124801e-02]]\n",
      "num = \n",
      "1322\n",
      "SENSOR 8 1.59030504807 -0.562457859173\n",
      "\n",
      "num = \n",
      "1323\n",
      "SENSOR 1 3.26977624121 1.02025691453\n",
      "\n",
      "num = \n",
      "1324\n",
      "SENSOR 4 3.98177703193 -2.11166828345\n",
      "\n",
      "num = \n",
      "1325\n",
      "SENSOR 7 3.60670130028 -0.68064217983\n",
      "\n",
      "num = \n",
      "1326\n",
      "ODOMETRY 0.000590926276013 0.100545969921 -0.199693808091\n",
      "\n",
      "sqP = \n",
      "[[1.391730694627939, 0.0, 0.0], [0.0011296957538760974, 0.26810440641108163, 0.0], [0.07188167253500995, 0.003698536994628647, 0.28466089672528677]]\n",
      "sigma = \n",
      "[[-0.6530871   5.29433915  1.50439952]\n",
      " [ 2.13037429  5.29433915  1.50439952]\n",
      " [-0.65082771  5.83054796  1.50439952]\n",
      " [-0.50932376  5.30173623  2.07372131]\n",
      " [-3.43654849  5.29433915  1.50439952]\n",
      " [-0.65534649  4.75813034  1.50439952]\n",
      " [-0.79685045  5.28694208  0.93507773]]\n",
      "sqP = \n",
      "[[1.4449164601756508, 0.0, 0.0], [-0.06342035785988152, 0.5220250805407958, 0.0], [0.06071095839068413, 0.005686239308727157, 0.5323388463713613]]\n",
      "m = \n",
      "[1.58923574 5.20769937 1.46564673]\n",
      "P = \n",
      "[[ 0.66122535 -0.06318795  0.04368128]\n",
      " [-0.06318795  0.07554991 -0.00506608]\n",
      " [ 0.04368128 -0.00506608  0.08327184]]\n",
      "num = \n",
      "1327\n",
      "SENSOR 8 1.42956896486 -0.405210773852\n",
      "\n",
      "num = \n",
      "1328\n",
      "SENSOR 1 3.11530262765 1.08617098938\n",
      "\n",
      "num = \n",
      "1329\n",
      "SENSOR 4 3.75792044617 -1.94960186471\n",
      "\n",
      "num = \n",
      "1330\n",
      "SENSOR 7 3.44632621596 -0.561375582074\n",
      "\n",
      "num = \n",
      "1331\n",
      "ODOMETRY -0.000298257136816 0.0996631664365 -0.198915852019\n",
      "\n",
      "sqP = \n",
      "[[0.8131576440357918, 0.0, 0.0], [-0.0777068902434053, 0.2636504346631028, 0.0], [0.05371808986858123, -0.0033825540433245396, 0.2835044321648166]]\n",
      "sigma = \n",
      "[[ 1.58923574  5.20769937  1.46564673]\n",
      " [ 3.21555103  5.20769937  1.46564673]\n",
      " [ 1.43382196  5.73500024  1.46564673]\n",
      " [ 1.69667192  5.20093426  2.0326556 ]\n",
      " [-0.03707955  5.20769937  1.46564673]\n",
      " [ 1.74464952  4.6803985   1.46564673]\n",
      " [ 1.48179956  5.21446448  0.89863787]]\n",
      "sqP = \n",
      "[[0.932817960817795, 0.0, 0.0], [-0.06686487332554047, 0.5213445272107067, 0.0], [0.015475245720219686, -0.0026910112225635714, 0.5311857309783561]]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "m = \n",
      "[1.8956121  5.14757884 1.59899082]\n",
      "P = \n",
      "[[ 0.22913497 -0.01497188  0.0015078 ]\n",
      " [-0.01497188  0.07693033 -0.00522592]\n",
      " [ 0.0015078  -0.00522592  0.07838534]]\n",
      "num = \n",
      "1332\n",
      "SENSOR 8 1.41119753152 -0.218291775829\n",
      "\n",
      "num = \n",
      "1333\n",
      "SENSOR 1 3.14171978452 1.37417197214\n",
      "\n",
      "num = \n",
      "1334\n",
      "SENSOR 4 3.97964357546 -1.84339806385\n",
      "\n",
      "num = \n",
      "1335\n",
      "SENSOR 7 3.37769084878 -0.326486102096\n",
      "\n",
      "num = \n",
      "1336\n",
      "ODOMETRY -0.000223643982462 0.0996312038275 -0.2025564277\n",
      "\n",
      "sqP = \n",
      "[[0.4786804418792084, 0.0, 0.0], [-0.03127740456151116, 0.2755940106657698, 0.0], [0.003149911012729152, -0.01860489672516714, 0.2793372108024022]]\n",
      "sigma = \n",
      "[[1.8956121  5.14757884 1.59899082]\n",
      " [2.85297299 5.14757884 1.59899082]\n",
      " [1.8330573  5.69876687 1.59899082]\n",
      " [1.90191193 5.11036905 2.15766524]\n",
      " [0.93825122 5.14757884 1.59899082]\n",
      " [1.95816691 4.59639082 1.59899082]\n",
      " [1.88931228 5.18478864 1.0403164 ]]\n",
      "sqP = \n",
      "[[0.6558377569286222, 0.0, 0.0], [-0.04156219328558243, 0.527013777883732, 0.0], [-0.008188735968550501, -0.009375873887449425, 0.5275305489749678]]\n",
      "m = \n",
      "[1.84131    5.06280589 1.67106368]\n",
      "P = \n",
      "[[ 0.15724918  0.00241884  0.00026366]\n",
      " [ 0.00241884  0.07471136 -0.00560308]\n",
      " [ 0.00026366 -0.00560308  0.07894508]]\n",
      "num = \n",
      "1337\n",
      "SENSOR 8 1.25280389716 -0.0478523885333\n",
      "\n",
      "num = \n",
      "1338\n",
      "SENSOR 1 3.28031792034 1.55461890617\n",
      "\n",
      "num = \n",
      "1339\n",
      "SENSOR 4 3.96871903979 -1.63320364602\n",
      "\n",
      "num = \n",
      "1340\n",
      "SENSOR 7 3.38773338161 -0.165645510408\n",
      "\n",
      "num = \n",
      "1341\n",
      "ODOMETRY -0.000881424513507 0.0999464043441 -0.148617707433\n",
      "\n",
      "sqP = \n",
      "[[0.39654657101525176, 0.0, 0.0], [0.006099760908610382, 0.2732657114013997, 0.0], [0.000664896545929946, -0.020518971215445825, 0.2802206469856885]]\n",
      "sigma = \n",
      "[[1.84131    5.06280589 1.67106368]\n",
      " [2.63440314 5.06280589 1.67106368]\n",
      " [1.85350952 5.60933731 1.67106368]\n",
      " [1.84263979 5.02176795 2.23150498]\n",
      " [1.04821686 5.06280589 1.67106368]\n",
      " [1.82911048 4.51627447 1.67106368]\n",
      " [1.83998021 5.10384383 1.11062239]]\n",
      "sqP = \n",
      "[[0.5977347506666424, 0.0, 0.0], [-0.019933219567863838, 0.5249405816397639, 0.0], [-0.005977519147831062, -0.010761589258245977, 0.5278312647697454]]\n",
      "m = \n",
      "[1.69943918 5.08948433 1.74954072]\n",
      "P = \n",
      "[[ 0.14763662  0.00695291 -0.00041564]\n",
      " [ 0.00695291  0.07381706 -0.00748583]\n",
      " [-0.00041564 -0.00748583  0.07917909]]\n",
      "num = \n",
      "1342\n",
      "SENSOR 8 1.21794802013 0.101499246355\n",
      "\n",
      "num = \n",
      "1343\n",
      "SENSOR 1 3.22237509743 1.79072716463\n",
      "\n",
      "num = \n",
      "1344\n",
      "SENSOR 4 3.95617109814 -1.44625205115\n",
      "\n",
      "num = \n",
      "1345\n",
      "SENSOR 7 3.44736574276 0.0259459077986\n",
      "\n",
      "num = \n",
      "1346\n",
      "ODOMETRY 6.73076006284e-05 0.100132073471 -0.000119445516902\n",
      "\n",
      "sqP = \n",
      "[[0.38423511175308045, 0.0, 0.0], [0.018095468563325556, 0.2710896864911864, 0.0], [-0.001081723437478595, -0.027541656546815875, 0.2800346061363935]]\n",
      "sigma = \n",
      "[[1.69943918 5.08948433 1.74954072]\n",
      " [2.4679094  5.08948433 1.74954072]\n",
      " [1.73563012 5.6316637  1.74954072]\n",
      " [1.69727573 5.03440101 2.30960993]\n",
      " [0.93096896 5.08948433 1.74954072]\n",
      " [1.66324824 4.54730495 1.74954072]\n",
      " [1.70160263 5.14456764 1.18947151]]\n",
      "sqP = \n",
      "[[0.5898857831919868, 0.0, 0.0], [-0.014128527904966248, 0.5237009687956486, 0.0], [-0.0034032619633866715, -0.01462532553455158, 0.5276045337670726]]\n",
      "m = \n",
      "[1.55504613 5.08292747 1.76214035]\n",
      "P = \n",
      "[[ 0.14648083  0.00780948  0.0013063 ]\n",
      " [ 0.00780948  0.07339851 -0.00724727]\n",
      " [ 0.0013063  -0.00724727  0.07954731]]\n",
      "num = \n",
      "1347\n",
      "SENSOR 8 1.10085634846 0.0128641004662\n",
      "\n",
      "num = \n",
      "1348\n",
      "SENSOR 1 3.15319314656 1.83602899903\n",
      "\n",
      "num = \n",
      "1349\n",
      "SENSOR 4 3.96197077783 -1.56149331217\n",
      "\n",
      "num = \n",
      "1350\n",
      "SENSOR 7 3.16144997508 0.0497659131117\n",
      "\n",
      "num = \n",
      "1351\n",
      "ODOMETRY 0.000216898596734 0.100008870944 8.44491124946e-05\n",
      "\n",
      "sqP = \n",
      "[[0.38272814406312744, 0.0, 0.0], [0.02040477018159753, 0.2701520991626242, 0.0], [0.003413120488988499, -0.027084420390543193, 0.2807171075515937]]\n",
      "sigma = \n",
      "[[1.55504613 5.08292747 1.76214035]\n",
      " [2.32050242 5.08292747 1.76214035]\n",
      " [1.59585567 5.62323167 1.76214035]\n",
      " [1.56187237 5.02875863 2.32357457]\n",
      " [0.78958984 5.08292747 1.76214035]\n",
      " [1.51423659 4.54262327 1.76214035]\n",
      " [1.54821989 5.13709631 1.20070614]]\n",
      "sqP = \n",
      "[[0.5889896741842274, 0.0, 0.0], [-0.013408957358191179, 0.5231235985140639, 0.0], [0.0019813560618984435, -0.014682107378899334, 0.5279506082602877]]\n",
      "m = \n",
      "[1.57815855 5.03749632 1.907527  ]\n",
      "P = \n",
      "[[ 0.14644827  0.00792074  0.00244942]\n",
      " [ 0.00792074  0.07333005 -0.0072752 ]\n",
      " [ 0.00244942 -0.0072752   0.07962699]]\n",
      "num = \n",
      "1352\n",
      "SENSOR 8 0.98718385021 0.0505647532356\n",
      "\n",
      "num = \n",
      "1353\n",
      "SENSOR 1 3.44793567727 1.89256285855\n",
      "\n",
      "num = \n",
      "1354\n",
      "SENSOR 4 3.92237270173 -1.60631882591\n",
      "\n",
      "num = \n",
      "1355\n",
      "SENSOR 7 3.21973581934 -0.0340907108888\n",
      "\n",
      "num = \n",
      "1356\n",
      "ODOMETRY 0.000128009797301 0.100043460223 0.000987839174826\n",
      "\n",
      "sqP = \n",
      "[[0.3826856013551929, 0.0, 0.0], [0.020697765212358884, 0.27000306120851997, 0.0], [0.006400594564590829, -0.027435536316008327, 0.2807727037852553]]\n",
      "sigma = \n",
      "[[1.57815855 5.03749632 1.907527  ]\n",
      " [2.34352975 5.03749632 1.907527  ]\n",
      " [1.61955408 5.57750244 1.907527  ]\n",
      " [1.59095974 4.98262525 2.46907241]\n",
      " [0.81278735 5.03749632 1.907527  ]\n",
      " [1.53676302 4.4974902  1.907527  ]\n",
      " [1.56535736 5.09236739 1.34598159]]\n",
      "sqP = \n",
      "[[0.5889971431001614, 0.0, 0.0], [-0.013397663757005093, 0.5230653271175941, 0.0], [0.002879941550754949, -0.014989247069967505, 0.5279613593828729]]\n",
      "m = \n",
      "[1.63273333 5.12694585 1.96821007]\n",
      "P = \n",
      "[[ 0.15315741  0.00546221  0.01010805]\n",
      " [ 0.00546221  0.07516232 -0.01097459]\n",
      " [ 0.01010805 -0.01097459  0.0775801 ]]\n",
      "num = \n",
      "1357\n",
      "SENSOR 8 0.89829332316 0.135383956612\n",
      "\n",
      "num = \n",
      "1358\n",
      "SENSOR 1 3.27245387945 1.88010743562\n",
      "\n",
      "num = \n",
      "1359\n",
      "SENSOR 4 3.96502932983 -1.56994091616\n",
      "\n",
      "num = \n",
      "1360\n",
      "SENSOR 7 2.97710577284 0.0715555315754\n",
      "\n",
      "num = \n",
      "1361\n",
      "ODOMETRY -0.000150318169053 0.100071664514 0.000144679939358\n",
      "\n",
      "sqP = \n",
      "[[0.39135330328103907, 0.0, 0.0], [0.013957223545116778, 0.27380196104002025, 0.0], [0.02582844318866754, -0.041398843472663635, 0.2742245913634458]]\n",
      "sigma = \n",
      "[[1.63273333 5.12694585 1.96821007]\n",
      " [2.41543994 5.12694585 1.96821007]\n",
      " [1.66064778 5.67454977 1.96821007]\n",
      " [1.68439022 5.04414816 2.51665926]\n",
      " [0.85002672 5.12694585 1.96821007]\n",
      " [1.60481888 4.57934192 1.96821007]\n",
      " [1.58107644 5.20974353 1.41976089]]\n",
      "sqP = \n",
      "[[0.594995227338522, 0.0, 0.0], [-0.018676345924669612, 0.5263380175759449, 0.0], [0.01045585202118782, -0.022444745428228673, 0.5240807832580541]]\n",
      "m = \n",
      "[1.62593565 5.061082   2.10257166]\n",
      "P = \n",
      "[[ 0.1526603   0.00611836  0.01098004]\n",
      " [ 0.00611836  0.07441814 -0.00987016]\n",
      " [ 0.01098004 -0.00987016  0.08104577]]\n",
      "num = \n",
      "1362\n",
      "SENSOR 8 0.774339649168 -0.000516516086974\n",
      "\n",
      "num = \n",
      "1363\n",
      "SENSOR 1 3.48116649203 1.98621024178\n",
      "\n",
      "num = \n",
      "1364\n",
      "SENSOR 4 3.85294043277 -1.65568756419\n",
      "\n",
      "num = \n",
      "1365\n",
      "SENSOR 7 2.90643136834 0.027023615806\n",
      "\n",
      "num = \n",
      "1366\n",
      "ODOMETRY -0.000465690711615 0.100017674194 -0.000142742436314\n",
      "\n",
      "sqP = \n",
      "[[0.3907176717049334, 0.0, 0.0], [0.015659274321518184, 0.27234707790752355, 0.0], [0.02810222906724825, -0.0378569139507405, 0.28075413907626756]]\n",
      "sigma = \n",
      "[[1.62593565 5.061082   2.10257166]\n",
      " [2.40737099 5.061082   2.10257166]\n",
      " [1.6572542  5.60577616 2.10257166]\n",
      " [1.68214011 4.98536817 2.66407994]\n",
      " [0.84450031 5.061082   2.10257166]\n",
      " [1.5946171  4.51638785 2.10257166]\n",
      " [1.56973119 5.13679583 1.54106339]]\n",
      "sqP = \n",
      "[[0.594722832194417, 0.0, 0.0], [-0.017908555050800218, 0.525256734675088, 0.0], [0.011984350969075453, -0.02124545212903214, 0.5275431057511261]]\n",
      "m = \n",
      "[1.64932842 5.13854858 2.20803358]\n",
      "P = \n",
      "[[ 0.1549937   0.00547225  0.01320487]\n",
      " [ 0.00547225  0.07596896 -0.01293694]\n",
      " [ 0.01320487 -0.01293694  0.08415029]]\n",
      "num = \n",
      "1367\n",
      "SENSOR 8 0.527252645966 0.172607270039\n",
      "\n",
      "num = \n",
      "1368\n",
      "SENSOR 1 3.3814040867 1.97634147246\n",
      "\n",
      "num = \n",
      "1369\n",
      "SENSOR 4 4.10861734812 -1.59369624478\n",
      "\n",
      "num = \n",
      "1370\n",
      "SENSOR 7 2.76353072157 0.00840562337829\n",
      "\n",
      "num = \n",
      "1371\n",
      "ODOMETRY -0.000128208315622 0.0999656883624 0.000594809485977\n",
      "\n",
      "sqP = \n",
      "[[0.39369239198498945, 0.0, 0.0], [0.013899807892146804, 0.27527395697490914, 0.0], [0.03354108702583952, -0.04869023253117163, 0.2839974405896754]]\n",
      "sigma = \n",
      "[[1.64932842 5.13854858 2.20803358]\n",
      " [2.4367132  5.13854858 2.20803358]\n",
      " [1.67712804 5.68909649 2.20803358]\n",
      " [1.71641059 5.04116811 2.77602847]\n",
      " [0.86194364 5.13854858 2.20803358]\n",
      " [1.6215288  4.58800067 2.20803358]\n",
      " [1.58224625 5.23592904 1.6400387 ]]\n",
      "sqP = \n",
      "[[0.5969186784522462, 0.0, 0.0], [-0.01984350299278799, 0.5278039959335232, 0.0], [0.014118251482095912, -0.02733570719109082, 0.5288929174619847]]\n",
      "m = \n",
      "[1.6300118  5.00945762 2.3754791 ]\n",
      "P = \n",
      "[[ 0.15477599  0.00636505  0.01390941]\n",
      " [ 0.00636505  0.07537768 -0.01229586]\n",
      " [ 0.01390941 -0.01229586  0.08874598]]\n",
      "num = \n",
      "1372\n",
      "SENSOR 8 0.687305913164 0.0798724661809\n",
      "\n",
      "num = \n",
      "1373\n",
      "SENSOR 1 3.21764284755 1.9744178959\n",
      "\n",
      "num = \n",
      "1374\n",
      "SENSOR 4 4.08919040605 -1.62141915547\n",
      "\n",
      "num = \n",
      "1375\n",
      "SENSOR 7 2.55473025478 0.0977268515407\n",
      "\n",
      "num = \n",
      "1376\n",
      "ODOMETRY 0.000821476656511 0.0999158037258 -0.00068566679204\n",
      "\n",
      "sqP = \n",
      "[[0.39341579965395984, 0.0, 0.0], [0.016178926717093454, 0.2740728370991853, 0.0], [0.03535549184539041, -0.046950579908578045, 0.2920472724656608]]\n",
      "sigma = \n",
      "[[1.6300118  5.00945762 2.3754791 ]\n",
      " [2.4168434  5.00945762 2.3754791 ]\n",
      " [1.66236965 5.5576033  2.3754791 ]\n",
      " [1.70072278 4.91555646 2.95957364]\n",
      " [0.8431802  5.00945762 2.3754791 ]\n",
      " [1.59765395 4.46131195 2.3754791 ]\n",
      " [1.55930081 5.10335878 1.79138455]]\n",
      "sqP = \n",
      "[[0.596898450233129, 0.0, 0.0], [-0.018892162458215435, 0.5269473405805706, 0.0], [0.0159060145967979, -0.02731191937303632, 0.5332311662463319]]\n",
      "m = \n",
      "[1.62258849 4.96778124 2.44570027]\n",
      "P = \n",
      "[[ 0.16285642  0.01138546 -0.01981477]\n",
      " [ 0.01138546  0.07544406 -0.00551407]\n",
      " [-0.01981477 -0.00551407  0.09198085]]\n",
      "num = \n",
      "1377\n",
      "SENSOR 8 0.480616169488 0.139441754953\n",
      "\n",
      "num = \n",
      "1378\n",
      "SENSOR 1 3.45668051094 2.00189239553\n",
      "\n",
      "num = \n",
      "1379\n",
      "SENSOR 4 4.10524461182 -1.67981251381\n",
      "\n",
      "num = \n",
      "1380\n",
      "SENSOR 7 2.33739775251 0.0985219039275\n",
      "\n",
      "num = \n",
      "1381\n",
      "ODOMETRY 0.000455722461866 0.0999653812 -4.23758429747e-05\n",
      "\n",
      "sqP = \n",
      "[[0.4035547331641658, 0.0, 0.0], [0.02821291822259343, 0.2732180242132194, 0.0], [-0.04910057632940192, -0.015111749061452434, 0.29890069132018415]]\n",
      "sigma = \n",
      "[[1.62258849 4.96778124 2.44570027]\n",
      " [2.42969796 4.96778124 2.44570027]\n",
      " [1.67901433 5.51421728 2.44570027]\n",
      " [1.52438734 4.93755774 3.04350165]\n",
      " [0.81547903 4.96778124 2.44570027]\n",
      " [1.56616266 4.42134519 2.44570027]\n",
      " [1.72078965 4.99800473 1.84789888]]\n",
      "sqP = \n",
      "[[0.6050316173636838, 0.0, 0.0], [-0.010596218250535097, 0.5238413153955078, 0.0], [-0.02555044444545523, -0.0063171576914510105, 0.5375701781685392]]\n",
      "m = \n",
      "[1.6041533  5.02584313 2.63574936]\n",
      "P = \n",
      "[[ 0.16763695  0.01304353 -0.0280752 ]\n",
      " [ 0.01304353  0.07616585 -0.00522676]\n",
      " [-0.0280752  -0.00522676  0.09393133]]\n",
      "num = \n",
      "1382\n",
      "SENSOR 8 0.400741221794 0.167751600491\n",
      "\n",
      "num = \n",
      "1383\n",
      "SENSOR 1 3.55108397687 2.05110783316\n",
      "\n",
      "num = \n",
      "1384\n",
      "SENSOR 4 4.06515935986 -1.72057455144\n",
      "\n",
      "num = \n",
      "1385\n",
      "SENSOR 7 2.50287051778 0.127838872546\n",
      "\n",
      "num = \n",
      "1386\n",
      "ODOMETRY 0.000100633095148 0.100219327349 0.000631070639127\n",
      "\n",
      "sqP = \n",
      "[[0.4094349198560882, 0.0, 0.0], [0.0318573955145107, 0.27413675142098687, 0.0], [-0.06857059542307656, -0.011097659844380934, 0.29850669800503776]]\n",
      "sigma = \n",
      "[[1.6041533  5.02584313 2.63574936]\n",
      " [2.42302314 5.02584313 2.63574936]\n",
      " [1.66786809 5.57411663 2.63574936]\n",
      " [1.46701211 5.00364781 3.23276275]\n",
      " [0.78528346 5.02584313 2.63574936]\n",
      " [1.5404385  4.47756962 2.63574936]\n",
      " [1.74129449 5.04803845 2.03873596]]\n",
      "sqP = \n",
      "[[0.6110268192176221, 0.0, 0.0], [-0.009841924706707477, 0.5240896273717257, 0.0], [-0.03435748864874163, -0.0030597740723718077, 0.5368795020595326]]\n",
      "m = \n",
      "[1.64904462 5.0705888  2.64958669]\n",
      "P = \n",
      "[[ 0.16944052  0.01414712 -0.02865867]\n",
      " [ 0.01414712  0.07625985 -0.00237505]\n",
      " [-0.02865867 -0.00237505  0.09535456]]\n",
      "num = \n",
      "1387\n",
      "SENSOR 8 0.417382565095 0.188823271199\n",
      "\n",
      "num = \n",
      "1388\n",
      "SENSOR 1 3.35123641832 2.09116664517\n",
      "\n",
      "num = \n",
      "1389\n",
      "SENSOR 4 3.95754572184 -1.71268379099\n",
      "\n",
      "num = \n",
      "1390\n",
      "SENSOR 7 2.41072585607 0.122504676512\n",
      "\n",
      "num = \n",
      "1391\n",
      "ODOMETRY -0.000607763710215 0.100140453337 -0.000218697666427\n",
      "\n",
      "sqP = \n",
      "[[0.41163153629450633, 0.0, 0.0], [0.03436841575724184, 0.2740048606402478, 0.0], [-0.06962215235361914, 6.480230464875384e-05, 0.3008443252690056]]\n",
      "sigma = \n",
      "[[1.64904462 5.0705888  2.64958669]\n",
      " [2.47230769 5.0705888  2.64958669]\n",
      " [1.71778145 5.61859852 2.64958669]\n",
      " [1.50980031 5.07071841 3.25127534]\n",
      " [0.82578154 5.0705888  2.64958669]\n",
      " [1.58030778 4.52257908 2.64958669]\n",
      " [1.78828892 5.0704592  2.04789804]]\n",
      "sqP = \n",
      "[[0.6127552152141903, 0.0, 0.0], [-0.010230227821065247, 0.5239254071443061, 0.0], [-0.036191543143889886, 0.0033531316203617378, 0.5382468040506473]]\n",
      "m = \n",
      "[1.71842247 5.0294869  2.6788888 ]\n",
      "P = \n",
      "[[ 0.157255    0.01648528 -0.00452214]\n",
      " [ 0.01648528  0.07776549 -0.0153034 ]\n",
      " [-0.00452214 -0.0153034   0.0962731 ]]\n",
      "num = \n",
      "1392\n",
      "SENSOR 8 0.369346735302 0.271181092817\n",
      "\n",
      "num = \n",
      "1393\n",
      "SENSOR 1 3.67699620635 2.07721447415\n",
      "\n",
      "num = \n",
      "1394\n",
      "SENSOR 4 4.07710876539 -1.73456190265\n",
      "\n",
      "num = \n",
      "1395\n",
      "SENSOR 7 2.42576722134 -0.0138112591739\n",
      "\n",
      "num = \n",
      "1396\n",
      "ODOMETRY -0.000139407170528 0.0998511677938 0.000334065170192\n",
      "\n",
      "sqP = \n",
      "[[0.3965539103522544, 0.0, 0.0], [0.04157134360297661, 0.2757486326704298, 0.0], [-0.011403604346738481, -0.0537784709854488, 0.30537015945853935]]\n",
      "sigma = \n",
      "[[1.71842247 5.0294869  2.6788888 ]\n",
      " [2.51153029 5.0294869  2.6788888 ]\n",
      " [1.80156516 5.58098416 2.6788888 ]\n",
      " [1.69561526 4.92192996 3.28962912]\n",
      " [0.92531465 5.0294869  2.6788888 ]\n",
      " [1.63527979 4.47798963 2.6788888 ]\n",
      " [1.74122968 5.13704384 2.06814848]]\n",
      "sqP = \n",
      "[[0.5992605632082905, 0.0, 0.0], [-0.0035085900119811086, 0.5270620755893596, 0.0], [-0.009314903460459937, -0.030592168637646706, 0.5408517481188837]]\n",
      "m = \n",
      "[1.7869928  5.07468476 2.76138787]\n",
      "P = \n",
      "[[ 0.15381289  0.0163028   0.0008803 ]\n",
      " [ 0.0163028   0.07864245 -0.01822044]\n",
      " [ 0.0008803  -0.01822044  0.09656666]]\n",
      "num = \n",
      "1397\n",
      "SENSOR 8 0.335693906267 0.462318775119\n",
      "\n",
      "num = \n",
      "1398\n",
      "SENSOR 1 3.54703844972 2.08539235736\n",
      "\n",
      "num = \n",
      "1399\n",
      "SENSOR 4 3.94824350404 -1.6777048794\n",
      "\n",
      "num = \n",
      "1400\n",
      "SENSOR 7 2.13297568426 0.0826896741318\n",
      "\n",
      "num = \n",
      "1401\n",
      "ODOMETRY -0.000628418701319 0.0999704949615 -0.000124838470818\n",
      "\n",
      "sqP = \n",
      "[[0.39218986442984555, 0.0, 0.0], [0.0415686498332329, 0.27733463027123856, 0.0], [0.0022445715655945333, -0.06603481376844746, 0.3036462252723733]]\n",
      "sigma = \n",
      "[[1.7869928  5.07468476 2.76138787]\n",
      " [2.57137253 5.07468476 2.76138787]\n",
      " [1.8701301  5.62935402 2.76138787]\n",
      " [1.79148194 4.94261513 3.36868032]\n",
      " [1.00261307 5.07468476 2.76138787]\n",
      " [1.7038555  4.5200155  2.76138787]\n",
      " [1.78250365 5.20675439 2.15409542]]\n",
      "sqP = \n",
      "[[0.5962766811740768, 0.0, 0.0], [-0.003854429014112782, 0.5293895707124985, 0.0], [-0.0038918919948883537, -0.038053504966658665, 0.5393678299219627]]\n",
      "m = \n",
      "[1.63532596 5.07628196 2.86740114]\n",
      "P = \n",
      "[[ 0.15820001  0.01079613  0.01740495]\n",
      " [ 0.01079613  0.07665265 -0.0166299 ]\n",
      " [ 0.01740495 -0.0166299   0.09937058]]\n",
      "num = \n",
      "1402\n",
      "SENSOR 8 0.0237407258792 0.999084065632\n",
      "\n",
      "num = \n",
      "1403\n",
      "SENSOR 1 3.49005275028 2.05858832887\n",
      "\n",
      "num = \n",
      "1404\n",
      "SENSOR 4 4.07307706178 -1.72646904791\n",
      "\n",
      "num = \n",
      "1405\n",
      "SENSOR 7 2.07921259872 0.00213504582166\n",
      "\n",
      "num = \n",
      "1406\n",
      "ODOMETRY -9.78009794874e-05 0.100003974246 -0.00134217319887\n",
      "\n",
      "sqP = \n",
      "[[0.39774365357993585, 0.0, 0.0], [0.027143432030108293, 0.27552837611012876, 0.0], [0.04375921829507317, -0.06466728418245137, 0.30540768581814703]]\n",
      "sigma = \n",
      "[[1.63532596 5.07628196 2.86740114]\n",
      " [2.43081327 5.07628196 2.86740114]\n",
      " [1.68961282 5.62733871 2.86740114]\n",
      " [1.7228444  4.94694739 3.47821651]\n",
      " [0.83983865 5.07628196 2.86740114]\n",
      " [1.58103909 4.52522521 2.86740114]\n",
      " [1.54780752 5.20561653 2.25658577]]\n",
      "sqP = \n",
      "[[0.6007092883011591, 0.0, 0.0], [-0.016290105705634, 0.52934020187251, 0.0], [0.02071010881483095, -0.039108185621860615, 0.5397374025384665]]\n",
      "m = \n",
      "[1.70800133 5.02102916 2.95247373]\n",
      "P = \n",
      "[[ 0.15721397  0.01606516  0.00554057]\n",
      " [ 0.01606516  0.07699276 -0.01772764]\n",
      " [ 0.00554057 -0.01772764  0.09843015]]\n",
      "num = \n",
      "1407\n",
      "SENSOR 8 0.129747875719 2.20808064967\n",
      "\n",
      "num = \n",
      "1408\n",
      "SENSOR 1 3.82990788235 2.17088111054\n",
      "\n",
      "num = \n",
      "1409\n",
      "SENSOR 4 4.01274770551 -1.80022287123\n",
      "\n",
      "num = \n",
      "1410\n",
      "SENSOR 7 1.74885650734 -0.0121223549881\n",
      "\n",
      "num = \n",
      "1411\n",
      "ODOMETRY 0.000936885880489 0.0999064874362 0.000600172698976\n",
      "\n",
      "sqP = \n",
      "[[0.39650216648992626, 0.0, 0.0], [0.040517217445543714, 0.27450156801816544, 0.0], [0.013973624473929684, -0.06664373529370027, 0.30625723969825447]]\n",
      "sigma = \n",
      "[[1.70800133 5.02102916 2.95247373]\n",
      " [2.50100566 5.02102916 2.95247373]\n",
      " [1.78903576 5.5700323  2.95247373]\n",
      " [1.73594857 4.88774169 3.5649882 ]\n",
      " [0.91499699 5.02102916 2.95247373]\n",
      " [1.62696689 4.47202603 2.95247373]\n",
      " [1.68005408 5.15431663 2.33995925]]\n",
      "sqP = \n",
      "[[0.5992085405947357, 0.0, 0.0], [-0.006648270682955304, 0.5282026751714016, 0.0], [0.003878165051250268, -0.039370229471367424, 0.5406458404947319]]\n",
      "m = \n",
      "[1.75450481 5.10643323 3.18895462]\n",
      "P = \n",
      "[[ 0.09681343 -0.00421646 -0.00657119]\n",
      " [-0.00421646  0.07724264 -0.01596543]\n",
      " [-0.00657119 -0.01596543  0.10087402]]\n",
      "num = \n",
      "1412\n",
      "SENSOR 8 0.00599324163092 2.67223936752\n",
      "\n",
      "num = \n",
      "1413\n",
      "SENSOR 1 3.82421795548 2.19552984414\n",
      "\n",
      "num = \n",
      "1414\n",
      "SENSOR 3 4.79851180987 0.60594351723\n",
      "\n",
      "num = \n",
      "1415\n",
      "SENSOR 4 4.05133471121 -1.81575940442\n",
      "\n",
      "num = \n",
      "1416\n",
      "SENSOR 7 1.85931144483 0.164227994673\n",
      "\n",
      "num = \n",
      "1417\n",
      "ODOMETRY -0.000253202498002 0.10008722051 0.000209063753791\n",
      "\n",
      "sqP = \n",
      "[[0.3111485737674717, 0.0, 0.0], [-0.01355127436632214, 0.27759503899576415, 0.0], [-0.021119140414881374, -0.058544361006520745, 0.31144913431335697]]\n",
      "sigma = \n",
      "[[1.75450481 5.10643323 3.18895462]\n",
      " [2.37680196 5.10643323 3.18895462]\n",
      " [1.72740227 5.66162331 3.18895462]\n",
      " [1.71226653 4.98934451 3.81185289]\n",
      " [1.13220767 5.10643323 3.18895462]\n",
      " [1.78160736 4.55124315 3.18895462]\n",
      " [1.79674309 5.22352195 2.56605635]]\n",
      "sqP = \n",
      "[[0.545383434004322, 0.0, 0.0], [-0.021073559375715232, 0.5304393525610585, 0.0], [-0.015131521564371819, -0.033402563133098646, 0.5440488514464967]]\n",
      "m = \n",
      "[1.75654322 5.16645529 3.25997023]\n",
      "P = \n",
      "[[ 0.09187853 -0.015025   -0.00821956]\n",
      " [-0.015025    0.06626433  0.00312498]\n",
      " [-0.00821956  0.00312498  0.0734621 ]]\n",
      "num = \n",
      "1418\n",
      "SENSOR 8 0.355098876696 2.89500927945\n",
      "\n",
      "num = \n",
      "1419\n",
      "SENSOR 1 3.90659962847 2.1333112384\n",
      "\n",
      "num = \n",
      "1420\n",
      "SENSOR 3 4.81068150996 0.62679112096\n",
      "\n",
      "num = \n",
      "1421\n",
      "SENSOR 4 4.09111006731 -1.88931307412\n",
      "\n",
      "num = \n",
      "1422\n",
      "SENSOR 7 1.79380510621 -0.0357020447254\n",
      "\n",
      "num = \n",
      "1423\n",
      "ODOMETRY 0.000157129099918 0.100032988722 -0.000729862285518\n",
      "\n",
      "sqP = \n",
      "[[0.30311472240159526, 0.0, 0.0], [-0.04956869633781998, 0.25260102125088985, 0.0], [-0.027117004505661294, 0.007049955486282141, 0.2695868343136981]]\n",
      "sigma = \n",
      "[[1.75654322 5.16645529 3.25997023]\n",
      " [2.36277267 5.16645529 3.25997023]\n",
      " [1.65740583 5.67165733 3.25997023]\n",
      " [1.70230922 5.1805552  3.79914389]\n",
      " [1.15031378 5.16645529 3.25997023]\n",
      " [1.85568062 4.66125325 3.25997023]\n",
      " [1.81077723 5.15235538 2.72079656]]\n",
      "sqP = \n",
      "[[0.5432043101141477, 0.0, 0.0], [-0.03961764458503843, 0.5153664146543263, 0.0], [-0.016500703945999336, 0.004545313782230044, 0.5222441025957609]]\n",
      "m = \n",
      "[1.85136673 5.21962482 3.3503551 ]\n",
      "P = \n",
      "[[ 0.09159897 -0.01832532 -0.00834536]\n",
      " [-0.01832532  0.06568587  0.00406179]\n",
      " [-0.00834536  0.00406179  0.07009885]]\n",
      "num = \n",
      "1424\n",
      "SENSOR 8 0.310596091371 2.80972354331\n",
      "\n",
      "num = \n",
      "1425\n",
      "SENSOR 1 3.70515903504 2.23033190653\n",
      "\n",
      "num = \n",
      "1426\n",
      "SENSOR 3 4.65126987644 0.73166186817\n",
      "\n",
      "num = \n",
      "1427\n",
      "SENSOR 4 4.23935852045 -1.93054147639\n",
      "\n",
      "num = \n",
      "1428\n",
      "SENSOR 7 1.76174667646 0.0262704139452\n",
      "\n",
      "num = \n",
      "1429\n",
      "ODOMETRY -0.000548408308394 0.0999919238562 -0.000250033911117\n",
      "\n",
      "sqP = \n",
      "[[0.3026532096590169, 0.0, 0.0], [-0.0605489130741139, 0.24903754931722824, 0.0], [-0.02757399179433491, 0.009605850597614892, 0.2631468239203823]]\n",
      "sigma = \n",
      "[[1.85136673 5.21962482 3.3503551 ]\n",
      " [2.45667315 5.21962482 3.3503551 ]\n",
      " [1.7302689  5.71769992 3.3503551 ]\n",
      " [1.79621874 5.23883652 3.87664875]\n",
      " [1.24606031 5.21962482 3.3503551 ]\n",
      " [1.97246455 4.72154972 3.3503551 ]\n",
      " [1.90651471 5.20041312 2.82406146]]\n",
      "sqP = \n",
      "[[0.544082219168055, 0.0, 0.0], [-0.04419321390889924, 0.5137064956229909, 0.0], [-0.0179473790521762, 0.005972263717314542, 0.5190462174757438]]\n",
      "m = \n",
      "[1.94365901 5.12869832 3.38996557]\n",
      "P = \n",
      "[[ 0.0925     -0.0194258  -0.00821359]\n",
      " [-0.0194258   0.06497974  0.00427941]\n",
      " [-0.00821359  0.00427941  0.06868909]]\n",
      "num = \n",
      "1430\n",
      "SENSOR 8 0.672273832166 3.01287570212\n",
      "\n",
      "num = \n",
      "1431\n",
      "SENSOR 1 3.82284974892 2.26851849591\n",
      "\n",
      "num = \n",
      "1432\n",
      "SENSOR 3 4.66661981658 0.750783246804\n",
      "\n",
      "num = \n",
      "1433\n",
      "SENSOR 4 4.20611841782 -1.87591401282\n",
      "\n",
      "num = \n",
      "1434\n",
      "SENSOR 7 1.65083446213 -0.10214389997\n",
      "\n",
      "num = \n",
      "1435\n",
      "ODOMETRY -0.000371751330884 0.0999313796848 0.000860224474773\n",
      "\n",
      "sqP = \n",
      "[[0.30413812603726476, 0.0, 0.0], [-0.06387163358734745, 0.24677955733288415, 0.0], [-0.027006105511559405, 0.010351301998592962, 0.26048533591624723]]\n",
      "sigma = \n",
      "[[1.94365901 5.12869832 3.38996557]\n",
      " [2.55193526 5.12869832 3.38996557]\n",
      " [1.81591574 5.62225743 3.38996557]\n",
      " [1.8896468  5.14940092 3.91093624]\n",
      " [1.33538276 5.12869832 3.38996557]\n",
      " [2.07140227 4.6351392  3.38996557]\n",
      " [1.99767122 5.10799571 2.8689949 ]]\n",
      "sqP = \n",
      "[[0.5452604102825337, 0.0, 0.0], [-0.0450841864385656, 0.5125548682480697, 0.0], [-0.019018691230955634, 0.006533597187752583, 0.5178075294261772]]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "m = \n",
      "[2.03367378 5.17670573 3.51010007]\n",
      "P = \n",
      "[[ 0.09422023 -0.01888046 -0.00933805]\n",
      " [-0.01888046  0.06590459  0.00333884]\n",
      " [-0.00933805  0.00333884  0.06613287]]\n",
      "num = \n",
      "1436\n",
      "SENSOR 8 0.556611108037 3.00403808456\n",
      "\n",
      "num = \n",
      "1437\n",
      "SENSOR 1 4.07609954779 2.29923731693\n",
      "\n",
      "num = \n",
      "1438\n",
      "SENSOR 3 4.61268174855 0.746177069223\n",
      "\n",
      "num = \n",
      "1439\n",
      "SENSOR 4 4.26511409215 -1.93342631232\n",
      "\n",
      "num = \n",
      "1440\n",
      "SENSOR 7 1.5749019735 0.109768246387\n",
      "\n",
      "num = \n",
      "1441\n",
      "ODOMETRY 2.93556084258e-05 0.100060189388 0.00098238378032\n",
      "\n",
      "sqP = \n",
      "[[0.3069531352480338, 0.0, 0.0], [-0.06150927283739107, 0.24924125683693038, 0.0], [-0.030421738336233878, 0.005888338062298485, 0.2552894783029187]]\n",
      "sigma = \n",
      "[[2.03367378 5.17670573 3.51010007]\n",
      " [2.64758005 5.17670573 3.51010007]\n",
      " [1.91065523 5.67518825 3.51010007]\n",
      " [1.9728303  5.18848241 4.02067902]\n",
      " [1.41976751 5.17670573 3.51010007]\n",
      " [2.15669233 4.67822322 3.51010007]\n",
      " [2.09451726 5.16492906 2.99952111]]\n",
      "sqP = \n",
      "[[0.546744090088755, 0.0, 0.0], [-0.043597973452639785, 0.5135517333583782, 0.0], [-0.021798217570683898, 0.004405108521893358, 0.5153570870825699]]\n",
      "m = \n",
      "[2.07308181 5.25623359 3.62090116]\n",
      "P = \n",
      "[[ 0.09520428 -0.0190901  -0.00910948]\n",
      " [-0.0190901   0.06520493  0.0033857 ]\n",
      " [-0.00910948  0.0033857   0.06351586]]\n",
      "num = \n",
      "1442\n",
      "SENSOR 8 0.731212089539 3.13442910945\n",
      "\n",
      "num = \n",
      "1443\n",
      "SENSOR 1 4.19104478917 2.19441500315\n",
      "\n",
      "num = \n",
      "1444\n",
      "SENSOR 3 4.50331304373 0.707514185573\n",
      "\n",
      "num = \n",
      "1445\n",
      "SENSOR 4 4.34762941195 -1.97920192321\n",
      "\n",
      "num = \n",
      "1446\n",
      "SENSOR 7 1.33294282082 0.0230575892315\n",
      "\n",
      "num = \n",
      "1447\n",
      "ODOMETRY 1.97994777393e-05 0.099913771488 0.000324267521873\n",
      "\n",
      "sqP = \n",
      "[[0.30855191047112623, 0.0, 0.0], [-0.06186997732509454, 0.24774389813330344, 0.0], [-0.029523329650127196, 0.006293143930538585, 0.2502091732438571]]\n",
      "sigma = \n",
      "[[2.07308181 5.25623359 3.62090116]\n",
      " [2.69018563 5.25623359 3.62090116]\n",
      " [1.94934185 5.75172138 3.62090116]\n",
      " [2.01403515 5.26881988 4.12131951]\n",
      " [1.45597798 5.25623359 3.62090116]\n",
      " [2.19682176 4.76074579 3.62090116]\n",
      " [2.13212846 5.2436473  3.12048282]]\n",
      "sqP = \n",
      "[[0.5476347346025631, 0.0, 0.0], [-0.0433763826282674, 0.5127802160702805, 0.0], [-0.021754881273539077, 0.004624359614959526, 0.5129073071846301]]\n",
      "m = \n",
      "[2.16725701 5.28225807 3.8026386 ]\n",
      "P = \n",
      "[[ 0.09546439 -0.01956553 -0.0073427 ]\n",
      " [-0.01956553  0.06402777  0.00360323]\n",
      " [-0.0073427   0.00360323  0.06180411]]\n",
      "num = \n",
      "1448\n",
      "SENSOR 8 0.726506378949 3.06634580351\n",
      "\n",
      "num = \n",
      "1449\n",
      "SENSOR 1 4.04114278918 2.33975587408\n",
      "\n",
      "num = \n",
      "1450\n",
      "SENSOR 3 4.50535713636 0.78187843772\n",
      "\n",
      "num = \n",
      "1451\n",
      "SENSOR 4 4.29965089498 -1.9407687623\n",
      "\n",
      "num = \n",
      "1452\n",
      "SENSOR 7 1.27944364032 0.0552717851415\n",
      "\n",
      "num = \n",
      "1453\n",
      "ODOMETRY -0.000438875804674 0.099903540815 0.000489960444763\n",
      "\n",
      "sqP = \n",
      "[[0.3089731250973052, 0.0, 0.0], [-0.06332437930707917, 0.24498529609869782, 0.0], [-0.02376486534039817, 0.008565163882532145, 0.2473175767033093]]\n",
      "sigma = \n",
      "[[2.16725701 5.28225807 3.8026386 ]\n",
      " [2.78520326 5.28225807 3.8026386 ]\n",
      " [2.04060825 5.77222866 3.8026386 ]\n",
      " [2.11972728 5.2993884  4.29727376]\n",
      " [1.54931076 5.28225807 3.8026386 ]\n",
      " [2.29390577 4.79228748 3.8026386 ]\n",
      " [2.21478674 5.26512774 3.30800345]]\n",
      "sqP = \n",
      "[[0.5477582841652594, 0.0, 0.0], [-0.04288890916327406, 0.5113386820742348, 0.0], [-0.020368487022879727, 0.005850891634579513, 0.5115637743107649]]\n",
      "m = \n",
      "[2.24700368 5.29091793 3.82042462]\n",
      "P = \n",
      "[[ 0.09584862 -0.01957779 -0.00417073]\n",
      " [-0.01957779  0.06356754  0.00329202]\n",
      " [-0.00417073  0.00329202  0.05799021]]\n",
      "num = \n",
      "1454\n",
      "SENSOR 8 0.62179737963 3.10384573817\n",
      "\n",
      "num = \n",
      "1455\n",
      "SENSOR 1 4.24151843231 2.38316523462\n",
      "\n",
      "num = \n",
      "1456\n",
      "SENSOR 3 4.26416968603 0.789449674711\n",
      "\n",
      "num = \n",
      "1457\n",
      "SENSOR 4 4.33240098534 -1.96547159788\n",
      "\n",
      "num = \n",
      "1458\n",
      "SENSOR 7 1.24198454896 -0.0312764055356\n",
      "\n",
      "num = \n",
      "1459\n",
      "ODOMETRY 0.00124083409796 0.09994562889 -0.000946313040158\n",
      "\n",
      "sqP = \n",
      "[[0.3095942778071431, 0.0, 0.0], [-0.0632369404697377, 0.24406686054842802, 0.0], [-0.013471613654760392, 0.00999774966677789, 0.24022649416786954]]\n",
      "sigma = \n",
      "[[2.24700368 5.29091793 3.82042462]\n",
      " [2.86619223 5.29091793 3.82042462]\n",
      " [2.1205298  5.77905165 3.82042462]\n",
      " [2.22006045 5.31091343 4.30087761]\n",
      " [1.62781512 5.29091793 3.82042462]\n",
      " [2.37347756 4.80278421 3.82042462]\n",
      " [2.27394691 5.27092243 3.33997163]]\n",
      "sqP = \n",
      "[[0.5477490409533937, 0.0, 0.0], [-0.041801195402017446, 0.5107869878928634, 0.0], [-0.016633287177267107, 0.006517016825750292, 0.5081114733392514]]\n",
      "m = \n",
      "[2.35058232 5.06471319 3.89112217]\n",
      "P = \n",
      "[[ 0.07850763  0.00491437 -0.00517691]\n",
      " [ 0.00491437  0.07484359  0.00860613]\n",
      " [-0.00517691  0.00860613  0.05744696]]\n",
      "num = \n",
      "1460\n",
      "SENSOR 8 0.760293224516 3.02148565162\n",
      "\n",
      "num = \n",
      "1461\n",
      "SENSOR 1 4.28994508486 2.2237346564\n",
      "\n",
      "num = \n",
      "1462\n",
      "SENSOR 3 4.26717238324 0.896293534553\n",
      "\n",
      "num = \n",
      "1463\n",
      "SENSOR 4 4.27865244716 -2.06179292551\n",
      "\n",
      "num = \n",
      "1464\n",
      "SENSOR 7 1.12266271613 0.0415462318364\n",
      "\n",
      "num = \n",
      "1465\n",
      "ODOMETRY 0.000129299270017 0.100041869119 -0.000655703667229\n",
      "\n",
      "sqP = \n",
      "[[0.2801921342477803, 0.0, 0.0], [0.0175392757787895, 0.2730127503882633, 0.0], [-0.018476288683492045, 0.032709777189728964, 0.23671852519924466]]\n",
      "sigma = \n",
      "[[2.35058232 5.06471319 3.89112217]\n",
      " [2.91096659 5.06471319 3.89112217]\n",
      " [2.38566087 5.6107387  3.89112217]\n",
      " [2.31362975 5.13013275 4.36455922]\n",
      " [1.79019805 5.06471319 3.89112217]\n",
      " [2.31550377 4.51868769 3.89112217]\n",
      " [2.3875349  4.99929364 3.41768512]]\n",
      "sqP = \n",
      "[[0.5283527529337633, 0.0, 0.0], [-0.002171588304480195, 0.5247742474943096, 0.0], [-0.018280829399703893, 0.015346662153307664, 0.5064149957305607]]\n",
      "m = \n",
      "[2.34750715 5.26488075 4.00651601]\n",
      "P = \n",
      "[[ 0.09410832 -0.01189467 -0.00800416]\n",
      " [-0.01189467  0.06623366  0.0008444 ]\n",
      " [-0.00800416  0.0008444   0.04926497]]\n",
      "num = \n",
      "1466\n",
      "SENSOR 1 4.45232713374 2.39852116851\n",
      "\n",
      "num = \n",
      "1467\n",
      "SENSOR 3 4.28823399159 0.800783040336\n",
      "\n",
      "num = \n",
      "1468\n",
      "SENSOR 4 4.51068601353 -2.05307475783\n",
      "\n",
      "num = \n",
      "1469\n",
      "SENSOR 7 0.901779605434 0.0337725706528\n",
      "\n",
      "num = \n",
      "1470\n",
      "SENSOR 8 0.977088757516 3.04355799617\n",
      "\n",
      "num = \n",
      "1471\n",
      "SENSOR 9 5.04440283106 -0.0103650380772\n",
      "\n",
      "num = \n",
      "1472\n",
      "ODOMETRY 0.000537988723435 0.0998524709068 -0.000469990594849\n",
      "\n",
      "sqP = \n",
      "[[0.3067708015765527, 0.0, 0.0], [-0.03877379379362157, 0.254421404140297, 0.0], [-0.0260916499670981, -0.0006574548154688947, 0.2204172563012601]]\n",
      "sigma = \n",
      "[[2.34750715 5.26488075 4.00651601]\n",
      " [2.96104875 5.26488075 4.00651601]\n",
      " [2.26995956 5.77372356 4.00651601]\n",
      " [2.29532385 5.26356584 4.44735052]\n",
      " [1.73396555 5.26488075 4.00651601]\n",
      " [2.42505474 4.75603794 4.00651601]\n",
      " [2.39969045 5.26619566 3.5656815 ]]\n",
      "sqP = \n",
      "[[0.5443275723156752, 0.0, 0.0], [-0.0299526143805431, 0.5154613205861116, 0.0], [-0.022225189059151253, 0.001327567799763482, 0.49934352229116485]]\n",
      "m = \n",
      "[2.3843322  5.10553027 4.11640251]\n",
      "P = \n",
      "[[ 0.07091904  0.00777117 -0.00119183]\n",
      " [ 0.00777117  0.07501912  0.0061102 ]\n",
      " [-0.00119183  0.0061102   0.04291553]]\n",
      "num = \n",
      "1473\n",
      "SENSOR 1 4.33243080487 2.33948279246\n",
      "\n",
      "num = \n",
      "1474\n",
      "SENSOR 3 4.10533364383 0.863859372451\n",
      "\n",
      "num = \n",
      "1475\n",
      "SENSOR 4 4.60752865229 -2.16099524972\n",
      "\n",
      "num = \n",
      "1476\n",
      "SENSOR 5 5.14915760553 -1.41243997907\n",
      "\n",
      "num = \n",
      "1477\n",
      "SENSOR 7 1.00236861922 0.0941916748689\n",
      "\n",
      "num = \n",
      "1478\n",
      "SENSOR 8 1.23019120912 3.1121174408\n",
      "\n",
      "num = \n",
      "1479\n",
      "SENSOR 9 4.82508017193 0.051751886788\n",
      "\n",
      "num = \n",
      "1480\n",
      "ODOMETRY -0.000205828817179 0.100018336841 0.000249542798678\n",
      "\n",
      "sqP = \n",
      "[[0.2663062870803106, 0.0, 0.0], [0.02918132523030959, 0.27233723937392956, 0.0], [-0.004475392194864741, 0.02291569124658288, 0.20584063881585873]]\n",
      "sigma = \n",
      "[[2.3843322  5.10553027 4.11640251]\n",
      " [2.91694477 5.10553027 4.11640251]\n",
      " [2.44269485 5.65020474 4.11640251]\n",
      " [2.37538141 5.15136165 4.52808379]\n",
      " [1.85171962 5.10553027 4.11640251]\n",
      " [2.32596955 4.56085579 4.11640251]\n",
      " [2.39328298 5.05969888 3.70472123]]\n",
      "sqP = \n",
      "[[0.5213354173811672, 0.0, 0.0], [0.0060417944555913716, 0.523361357011455, 0.0], [-0.011304242026893898, 0.008763791754935529, 0.4926060039264452]]\n",
      "m = \n",
      "[2.44405966 4.95815136 4.20792734]\n",
      "P = \n",
      "[[0.05624636 0.00939817 0.00288401]\n",
      " [0.00939817 0.055576   0.00125254]\n",
      " [0.00288401 0.00125254 0.03993932]]\n",
      "num = \n",
      "1481\n",
      "SENSOR 1 4.33283896214 2.46058071055\n",
      "\n",
      "num = \n",
      "1482\n",
      "SENSOR 3 4.18312790586 0.872387916195\n",
      "\n",
      "num = \n",
      "1483\n",
      "SENSOR 4 4.50465419667 -2.16382123772\n",
      "\n",
      "num = \n",
      "1484\n",
      "SENSOR 5 5.10336341986 -1.39939623537\n",
      "\n",
      "num = \n",
      "1485\n",
      "SENSOR 7 0.704062055735 0.108392416192\n",
      "\n",
      "num = \n",
      "1486\n",
      "SENSOR 8 1.20229516344 3.16630582891\n",
      "\n",
      "num = \n",
      "1487\n",
      "SENSOR 9 4.60881937906 0.0659418142304\n",
      "\n",
      "num = \n",
      "1488\n",
      "ODOMETRY -4.84769033309e-05 0.0998816841886 -0.000516660455681\n",
      "\n",
      "sqP = \n",
      "[[0.23716314737844202, 0.0, 0.0], [0.0396274356789727, 0.23239118178194168, 0.0], [0.012160456761201777, 0.0033161883976348416, 0.1994503673979439]]\n",
      "sigma = \n",
      "[[2.44405966 4.95815136 4.20792734]\n",
      " [2.91838595 4.95815136 4.20792734]\n",
      " [2.52331453 5.42293373 4.20792734]\n",
      " [2.46838057 4.96478374 4.60682808]\n",
      " [1.96973336 4.95815136 4.20792734]\n",
      " [2.36480478 4.493369   4.20792734]\n",
      " [2.41973874 4.95151899 3.80902661]]\n",
      "sqP = \n",
      "[[0.5079021252579878, 0.0, 0.0], [0.011141292375043273, 0.5029320293229589, 0.0], [-0.0036567008948666002, 0.00018376225374706413, 0.4896130884639111]]\n",
      "m = \n",
      "[2.52301624 4.98789145 4.28202577]\n",
      "P = \n",
      "[[0.05360034 0.00943543 0.00511183]\n",
      " [0.00943543 0.05511616 0.000249  ]\n",
      " [0.00511183 0.000249   0.03940954]]\n",
      "num = \n",
      "1489\n",
      "SENSOR 1 4.50934992142 2.36066884164\n",
      "\n",
      "num = \n",
      "1490\n",
      "SENSOR 3 4.00995160209 0.872702584178\n",
      "\n",
      "num = \n",
      "1491\n",
      "SENSOR 4 4.6556628157 -2.10991460657\n",
      "\n",
      "num = \n",
      "1492\n",
      "SENSOR 5 5.19236777304 -1.46894121337\n",
      "\n",
      "num = \n",
      "1493\n",
      "SENSOR 7 0.595140527124 -0.00247210771207\n",
      "\n",
      "num = \n",
      "1494\n",
      "SENSOR 8 1.1409026588 3.07525098835\n",
      "\n",
      "num = \n",
      "1495\n",
      "SENSOR 9 4.84639214222 -0.0548253969277\n",
      "\n",
      "num = \n",
      "1496\n",
      "ODOMETRY 5.67295197279e-05 0.100061981172 0.000311855228904\n",
      "\n",
      "sqP = \n",
      "[[0.2315174674299632, 0.0, 0.0], [0.04075473364595833, 0.23120382108902535, 0.0], [0.022079679082903356, -0.0028150746925597, 0.19726657402486109]]\n",
      "sigma = \n",
      "[[2.52301624 4.98789145 4.28202577]\n",
      " [2.98605118 4.98789145 4.28202577]\n",
      " [2.60452571 5.45029909 4.28202577]\n",
      " [2.5671756  4.9822613  4.67655892]\n",
      " [2.05998131 4.98789145 4.28202577]\n",
      " [2.44150678 4.52548381 4.28202577]\n",
      " [2.47885689 4.9935216  3.88749262]]\n",
      "sqP = \n",
      "[[0.5057161241923586, 0.0, 0.0], [0.012341379150226792, 0.5024097868445104, 0.0], [-5.2233212546030167e-05, -0.002651336907674354, 0.4884155040979234]]\n",
      "m = \n",
      "[2.49539681 4.88660345 4.27486315]\n",
      "P = \n",
      "[[ 0.05261094  0.00032047 -0.00105887]\n",
      " [ 0.00032047  0.04733827 -0.00547137]\n",
      " [-0.00105887 -0.00547137  0.0356208 ]]\n",
      "num = \n",
      "1497\n",
      "SENSOR 1 4.62274843795 2.4285094599\n",
      "\n",
      "num = \n",
      "1498\n",
      "SENSOR 3 3.816414436 0.903117907018\n",
      "\n",
      "num = \n",
      "1499\n",
      "SENSOR 4 4.36708385287 -2.09249135014\n",
      "\n",
      "num = \n",
      "1500\n",
      "SENSOR 5 5.02112185456 -1.48480085145\n",
      "\n",
      "num = \n",
      "1501\n",
      "SENSOR 7 0.722330360765 0.131951761725\n",
      "\n",
      "num = \n",
      "1502\n",
      "SENSOR 8 1.42401805985 3.15417376672\n",
      "\n",
      "num = \n",
      "1503\n",
      "SENSOR 9 4.47099457601 -0.0163240457546\n",
      "\n",
      "num = \n",
      "1504\n",
      "ODOMETRY 0.00115916483704 0.0999765525792 -5.49301680053e-05\n",
      "\n",
      "sqP = \n",
      "[[0.2293707580168494, 0.0, 0.0], [0.0013971716064254715, 0.21756912054247887, 0.0], [-0.004616419272187569, -0.02511810325495375, 0.18699884459366412]]\n",
      "sigma = \n",
      "[[2.49539681 4.88660345 4.27486315]\n",
      " [2.95413832 4.88660345 4.27486315]\n",
      " [2.49819115 5.32174169 4.27486315]\n",
      " [2.48616397 4.83636724 4.64886084]\n",
      " [2.03665529 4.88660345 4.27486315]\n",
      " [2.49260246 4.45146521 4.27486315]\n",
      " [2.50462965 4.93683965 3.90086546]]\n",
      "sqP = \n",
      "[[0.5026273053153982, 0.0, 0.0], [-0.005254720872209834, 0.4980761437914431, 0.0], [-0.009793307042829338, -0.009042052409524945, 0.48502488838846286]]\n",
      "m = \n",
      "[2.49859072 5.02999401 4.42113618]\n",
      "P = \n",
      "[[ 0.05180989  0.00794754 -0.00047703]\n",
      " [ 0.00794754  0.05441182  0.00045975]\n",
      " [-0.00047703  0.00045975  0.03639602]]\n",
      "num = \n",
      "1505\n",
      "SENSOR 1 4.63927554993 2.3761844012\n",
      "\n",
      "num = \n",
      "1506\n",
      "SENSOR 3 3.9376280419 0.858518143986\n",
      "\n",
      "num = \n",
      "1507\n",
      "SENSOR 4 4.78153044206 -2.1711021116\n",
      "\n",
      "num = \n",
      "1508\n",
      "SENSOR 5 5.01779109603 -1.4755448089\n",
      "\n",
      "num = \n",
      "1509\n",
      "SENSOR 7 0.400487152841 0.0954002442539\n",
      "\n",
      "num = \n",
      "1510\n",
      "SENSOR 8 1.5705356409 2.92820511279\n",
      "\n",
      "num = \n",
      "1511\n",
      "SENSOR 9 4.55151136998 0.00907004840239\n",
      "\n",
      "num = \n",
      "1512\n",
      "ODOMETRY -0.00015974321687 0.100001954963 0.000375007480502\n",
      "\n",
      "sqP = \n",
      "[[0.22761785600727102, 0.0, 0.0], [0.03491615982875159, 0.23063538555844101, 0.0], [-0.0020957435084861973, 0.0023106876302871667, 0.19075190412845]]\n",
      "sigma = \n",
      "[[2.49859072 5.02999401 4.42113618]\n",
      " [2.95382643 5.02999401 4.42113618]\n",
      " [2.56842304 5.49126478 4.42113618]\n",
      " [2.49439924 5.03461539 4.80263999]\n",
      " [2.04335501 5.02999401 4.42113618]\n",
      " [2.4287584  4.56872324 4.42113618]\n",
      " [2.50278221 5.02537264 4.03963237]]\n",
      "sqP = \n",
      "[[0.5030242725071893, 0.0, 0.0], [0.009899923240484757, 0.502321786652316, 0.0], [-0.008955450174687268, 0.0002741336825500636, 0.4865127526138337]]\n",
      "m = \n",
      "[2.44991942 4.92886529 4.5206732 ]\n",
      "P = \n",
      "[[ 0.05212928  0.00024379 -0.00245968]\n",
      " [ 0.00024379  0.04701552 -0.00558746]\n",
      " [-0.00245968 -0.00558746  0.03634784]]\n",
      "num = \n",
      "1513\n",
      "SENSOR 1 4.70778012293 2.42157051538\n",
      "\n",
      "num = \n",
      "1514\n",
      "SENSOR 3 3.97505453196 0.932447245049\n",
      "\n",
      "num = \n",
      "1515\n",
      "SENSOR 4 4.70850325866 -2.17152767966\n",
      "\n",
      "num = \n",
      "1516\n",
      "SENSOR 5 4.95124738508 -1.40632188062\n",
      "\n",
      "num = \n",
      "1517\n",
      "SENSOR 7 0.383618192004 0.149257977921\n",
      "\n",
      "num = \n",
      "1518\n",
      "SENSOR 8 1.42134250744 3.07398092328\n",
      "\n",
      "num = \n",
      "1519\n",
      "SENSOR 9 4.4606042563 -0.0161739269736\n",
      "\n",
      "num = \n",
      "1520\n",
      "ODOMETRY -0.000584883749866 0.0998918193766 0.000164194828446\n",
      "\n",
      "sqP = \n",
      "[[0.22831838035782145, 0.0, 0.0], [0.0010677754447147366, 0.2168280054754011, 0.0], [-0.010773046677697184, -0.02571602305320666, 0.18860134347644844]]\n",
      "sigma = \n",
      "[[2.44991942 4.92886529 4.5206732 ]\n",
      " [2.90655618 4.92886529 4.5206732 ]\n",
      " [2.45205497 5.36252131 4.5206732 ]\n",
      " [2.42837333 4.87743325 4.89787589]\n",
      " [1.99328266 4.92886529 4.5206732 ]\n",
      " [2.44778387 4.49520928 4.5206732 ]\n",
      " [2.47146551 4.98029734 4.14347051]]\n",
      "sqP = \n",
      "[[0.5022414573572547, 0.0, 0.0], [-0.005396184080453582, 0.4977867481753969, 0.0], [-0.011781603167915806, -0.00922782686305909, 0.48575339871023154]]\n",
      "m = \n",
      "[2.53479678 5.06156264 4.59187813]\n",
      "P = \n",
      "[[0.0514644  0.00673733 0.00379382]\n",
      " [0.00673733 0.05386816 0.0001967 ]\n",
      " [0.00379382 0.0001967  0.04177457]]\n",
      "num = \n",
      "1521\n",
      "SENSOR 1 4.79178293375 2.48566706534\n",
      "\n",
      "num = \n",
      "1522\n",
      "SENSOR 3 3.63177076576 0.880780831994\n",
      "\n",
      "num = \n",
      "1523\n",
      "SENSOR 4 5.01568989921 -2.26708689866\n",
      "\n",
      "num = \n",
      "1524\n",
      "SENSOR 5 4.98135878948 -1.51500189459\n",
      "\n",
      "num = \n",
      "1525\n",
      "SENSOR 7 0.270711489516 0.323697074147\n",
      "\n",
      "num = \n",
      "1526\n",
      "SENSOR 8 1.83521291936 3.07684872323\n",
      "\n",
      "num = \n",
      "1527\n",
      "SENSOR 9 4.49376890802 0.0459102111966\n",
      "\n",
      "num = \n",
      "1528\n",
      "ODOMETRY 0.000303680491478 0.100162153889 9.76063766532e-05\n",
      "\n",
      "sqP = \n",
      "[[0.226857655274443, 0.0, 0.0], [0.029698503416577614, 0.2301872326380464, 0.0], [0.01672334331578599, -0.0013030847151166858, 0.2036988119710624]]\n",
      "sigma = \n",
      "[[2.53479678 5.06156264 4.59187813]\n",
      " [2.98851209 5.06156264 4.59187813]\n",
      " [2.59419379 5.52193711 4.59187813]\n",
      " [2.56824347 5.05895647 4.99927575]\n",
      " [2.08108147 5.06156264 4.59187813]\n",
      " [2.47539978 4.60118818 4.59187813]\n",
      " [2.5013501  5.06416881 4.1844805 ]]\n",
      "sqP = \n",
      "[[0.5026192068290468, 0.0, 0.0], [0.007786951443527534, 0.5023179175043776, 0.0], [-0.001525545974257116, -0.0015631027803653297, 0.4911891620605785]]\n",
      "m = \n",
      "[2.48742541 4.89362019 4.74298105]\n",
      "P = \n",
      "[[ 5.20835990e-02 -5.43244705e-05 -2.03233769e-03]\n",
      " [-5.43244705e-05  4.64187832e-02 -5.67251693e-03]\n",
      " [-2.03233769e-03 -5.67251693e-03  3.74602935e-02]]\n",
      "num = \n",
      "1529\n",
      "SENSOR 1 4.87239258496 2.49373595981\n",
      "\n",
      "num = \n",
      "1530\n",
      "SENSOR 3 3.63961037672 0.972600323588\n",
      "\n",
      "num = \n",
      "1531\n",
      "SENSOR 4 5.06493994808 -2.19537045826\n",
      "\n",
      "num = \n",
      "1532\n",
      "SENSOR 5 4.87043159977 -1.5187934091\n",
      "\n",
      "num = \n",
      "1533\n",
      "SENSOR 7 0.158485679685 0.430892408212\n",
      "\n",
      "num = \n",
      "1534\n",
      "SENSOR 8 1.94404523721 3.02999537927\n",
      "\n",
      "num = \n",
      "1535\n",
      "SENSOR 9 4.25502389275 0.0807923140829\n",
      "\n",
      "num = \n",
      "1536\n",
      "ODOMETRY -0.000118594429945 0.100038226107 0.0016860488551\n",
      "\n",
      "sqP = \n",
      "[[0.2282183143323209, 0.0, 0.0], [-0.0002380372962354414, 0.21545005579814322, 0.0], [-0.008905234848857434, -0.026338525126889965, 0.19153921895416762]]\n",
      "sigma = \n",
      "[[2.48742541 4.89362019 4.74298105]\n",
      " [2.94386204 4.89362019 4.74298105]\n",
      " [2.48694934 5.3245203  4.74298105]\n",
      " [2.46961494 4.84094314 5.12605949]\n",
      " [2.03098879 4.89362019 4.74298105]\n",
      " [2.48790149 4.46272008 4.74298105]\n",
      " [2.50523588 4.94629724 4.35990262]]\n",
      "sqP = \n",
      "[[0.5021583006046537, 0.0, 0.0], [-0.005746580107882962, 0.49724245482517804, 0.0], [-0.011371928996465022, -0.009642689036662492, 0.4868787599422115]]\n",
      "m = \n",
      "[2.38704609 4.91377928 4.86335936]\n",
      "P = \n",
      "[[ 0.05120504 -0.00222219 -0.00259597]\n",
      " [-0.00222219  0.04501264 -0.00571393]\n",
      " [-0.00259597 -0.00571393  0.03836742]]\n",
      "num = \n",
      "1537\n",
      "SENSOR 3 3.85525315557 0.972472209298\n",
      "\n",
      "num = \n",
      "1538\n",
      "SENSOR 4 4.7395963428 -2.22231607732\n",
      "\n",
      "num = \n",
      "1539\n",
      "SENSOR 5 5.03393746044 -1.59221850796\n",
      "\n",
      "num = \n",
      "1540\n",
      "SENSOR 6 5.09689220959 -0.905474203378\n",
      "\n",
      "num = \n",
      "1541\n",
      "SENSOR 7 0.210352002822 1.03187235077\n",
      "\n",
      "num = \n",
      "1542\n",
      "SENSOR 8 2.18673487592 3.14059664668\n",
      "\n",
      "num = \n",
      "1543\n",
      "SENSOR 9 4.10712293299 0.0906600190361\n",
      "\n"
     ]
    }
   ],
   "source": [
    "# Начальные распределения\n",
    "m = []\n",
    "P = []\n",
    "m.append([0,0,0])\n",
    "P.append([[1,0,0],[0,1,0],[0,0,1]])\n",
    "Y = []\n",
    "f = open('/data files/sensor_data_ekf.dat')\n",
    "X = []\n",
    "m_ = []\n",
    "p_ = []\n",
    "R = []\n",
    "n = 3\n",
    "k = 0\n",
    "nn =0 \n",
    "alpha = 1\n",
    "beta = 0\n",
    "nums = []\n",
    "kappa = 1\n",
    "# Генерация сигма точек\n",
    "delta = alpha * alpha * ( n + kappa ) - n\n",
    "wm = []\n",
    "wm.append( delta / (delta + n))\n",
    "for i in range(1,2*n+1):\n",
    "    wm.append(1 / (2*(delta + n)))\n",
    "wc = []\n",
    "wc.append(delta / (delta + n) + (1 - alpha * alpha +  beta * beta))\n",
    "for i in range(1,2*n+1):\n",
    "    wc.append(1 / (2 * (delta +  n )))\n",
    "for line in f:\n",
    "    nn = nn+1\n",
    "    print(\"num = \")\n",
    "    print(nn)\n",
    "    print(line)\n",
    "    q = line.split()\n",
    "    if q[0] == 'ODOMETRY':\n",
    "        if(len(nums)!= 0):\n",
    "            # Новый прогноз\n",
    "            sigma = gensigma(n,m[-1],P[-1])\n",
    "            sigmax = np.array([[0.0]* n for i in range(0,2 * n + 1)])\n",
    "            for i in range(0,len(sigmax)):\n",
    "                sigma[i][0] = nrml(sigma[i][0])\n",
    "                sigmax[i] = np.array([sigma[i][0] + deltar1 + deltar2 , \n",
    "                                      sigma[i][1] + deltat * cos(sigma[i][0] + deltar1),\n",
    "                                      sigma[i][2] + deltat * sin(sigma[i][0] + deltar1)]) \n",
    "                sigmax[i][0] = nrml(sigmax[i][0])\n",
    "            m_ = [0.0] * n\n",
    "            for i in range(0,2*n+1):\n",
    "                m_ = m_ + wm[i] * sigmax[i]\n",
    "                m_[0] = nrml(m_[0])\n",
    "            # Прогноз матрицы ковариации\n",
    "            p_ =   np.array([[0.2,0,0],[0,0.2,0],[0,0,0.2]])\n",
    "            for i in range(0,2*n+1):\n",
    "                p_ = p_ + wc[i] * np.outer(np.array( sigmax[i]  - m_),np.array(sigmax[i] - m_))\n",
    "            \n",
    "            # Коррекция с конечной матрицей\n",
    "        \n",
    "            R = np.array([[0.0] * (2 * k)  for i in range(2 * k)])\n",
    "            for i in range(k):\n",
    "                R[2*i][2*i] = 0.2\n",
    "                R[2*i+1][2*i+1] = 1\n",
    "            sigma = gensigma(n,m_,p_)\n",
    "            mu =  np.array([0.0] * (2 * k))\n",
    "            S  =  R\n",
    "            C =  np.array([[0.0] * (2 * k)  for i in range(n)])\n",
    "            sigmax = np.array([[0.0] * (2 * k) for i in range(2 * n+1)])\n",
    "            for i in range(2 * n + 1):\n",
    "                for j in range(0,k):\n",
    "                    num = nums[j]\n",
    "                    delta_x = L[num - 1][1] - sigma[i][1]\n",
    "                    delta_y = L[num - 1][2] - sigma[i][2]\n",
    "                    qq = (delta_x * delta_x) + (delta_y * delta_y)\n",
    "                    phi = math.atan2(delta_y,delta_x)  - sigma[i][0]\n",
    "                    phi = nrml(phi)\n",
    "                    sigmax[i][2*j] = (math.sqrt(qq))\n",
    "                    sigmax[i][2*j+1] = (phi)\n",
    "                mu = mu + wm[i]  * sigmax[i]\n",
    "            for i in range(2*n+1):\n",
    "                S = S + wc[i] * np.outer(np.array(sigmax[i] - mu),     np.array(sigmax[i] - mu)) \n",
    "                C = C + wc[i] * np.outer(np.array(sigma[i]  - m_ ), np.array(sigmax[i] - mu))\n",
    "        \n",
    "            K = np.dot(C, np.linalg.inv(S))\n",
    "            v = np.array(Y) - np.array(mu)\n",
    "            v = np.dot(K , v.transpose())\n",
    "            v = v.tolist();\n",
    "            v[0] = nrml(v[0])\n",
    "            new_ang= m_+ v\n",
    "            new_ang[0] = nrml(new_ang[0])\n",
    "            m.append(new_ang)\n",
    "            P.append(p_  - np.dot(np.dot(K ,S),K.transpose()))\n",
    "      #      print(\"m = \")\n",
    "      #      print(m[-1])\n",
    "      #      print(\"P = \")\n",
    "      #      print(P[-1])\n",
    "        \n",
    "        #Берем дельты из Одометрии\n",
    "        deltar1 = float(q[1])\n",
    "        deltat = float(q[2])\n",
    "        deltar2 = float(q[3])\n",
    "        nums = []\n",
    "        Y = []\n",
    "        R = []\n",
    "        k = 0\n",
    "    else:\n",
    "        k = k+1\n",
    "        num = int(q[1])\n",
    "        nums.append(num)\n",
    "        Y.append(float(q[2]))\n",
    "        Y.append(float(q[3]))\n",
    "       "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "id": "efb0323e",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x7f54953e2ee0>]"
      ]
     },
     "execution_count": 24,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "X =[]\n",
    "for i in range(0,len(m)):\n",
    "    X.append(m[i])#np.random.multivariate_normal(m[i],P[i]))\n",
    "x = [a[1] for a in X]\n",
    "y = [a[2] for a in X]\n",
    "fig1 = plt.figure(1)\n",
    "axes1 = fig1.subplots(1, 1)\n",
    "axes1.plot(x, y,label='parametric curve')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2ddda105",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "709da174",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
